DIY Segway Code
I finally had some time to sit down and finish the code for my DIY Segway or “ChadWay” as it has been named around the Invention Studio. This code is developed for an Arduino Uno and uses the servo library to send motor drive commands to a Sabertooth 2×25 motor controller. I commented almost every line so it should be pretty easy to understand. If you have any questions, feel free to ask!
-Chad
#include <Servo.h> //includes the servo library in the code #include <Wire.h> //includes the servo library in the code #include <FreeSixIMU.h> //includes the servo library in the code #include <FIMU_ADXL345.h> //includes the servo library in the code #include <FIMU_ITG3200.h> //includes the servo library in the code #define KP 0.5 // proportional controller gain const int leftButtonPin = 2; //tells the arduino where the left steering button is const int rightButtonPin = 3; //tells the arduino where the right steering button is int leftButtonState = 0; //creates a variable for the left switch's state to be stored to int rightButtonState= 0; //creates a variables for the right switch's state to be stored to Servo leftMotor; //creates a servo instance for the left motor Servo rightMotor; //creates a servo instance for the left motor float angles[3]; //creates an array for the sensor data to go in FreeSixIMU sixDOF = FreeSixIMU(); void setup() { digitalWrite(10,LOW); //sets the pin to off digitalWrite(11,LOW); //sets the pin to off Serial.begin(9600); // sets the serial BAUD rate delay(5); sixDOF.init(); //initializes the sensor delay(5); pinMode(leftButtonPin, INPUT); //sets the port the button is plugged into as an input port pinMode(rightButtonPin, INPUT); //sets the port the button is plugged into as an input port Wire.begin(); //begins all the wire library stuff leftMotor.attach(10); rightMotor.attach(11); digitalWrite(10,LOW); //sets the pin to off digitalWrite(11,LOW); //sets the pin to off } void loop() { float corrected_angles = 0.00; float output = 0.00; signed int motorL = 0; signed int motorR = 0; sixDOF.getEuler(angles); //Gets the angles array from the sensors corrected_angles=(angles[1]); //The angle component from the array corrected_angles=corrected_angles-3; //Adjusts the value to compensate for the sensor being mounted nonlevel output=corrected_angles*KP; //Proportional control loop output=output-.20; //Cleaning things up abit (a gain adjust) motorL=map(output,-30,30,150,30); //Maps sensor values to motor outputs motorR=map(output,-30,30,150,30); //Maps sensor values to motor outputs leftBuuttonState = digitalRead(leftButtonPin); //Reads the state of the left steering button rightButtonState = digitalRead(rightButtonPin); //Reads the state of the right steering button if(leftButtonState == HIGH) { //If the state of the left steering button is high then tell motorL to go faster motorL=motorL+10; } if(rightButtonState == HIGH) { //If the state of the left steering button is high then tell motorR to go faster motorR=motorR+10; } leftMotor.write(motorL); //Writes speed value to the left motor rightMotor.write(motorR); //Writes speed value to the right motor }
