Archive

Tag Archives: steering

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
}

 

Advertisements