Monday 2 November 2009

Arduino code

I see some of you are interested in the code.. well here it is, unedited:

//------------------------------------------------------------

int xPin = 2; // select the input pin for the potentiometer
int gyroPin = 1;
int steerPin = 3;
int ledPin = 13; // select the pin for the LED
int pwmPinL = 9;
int pwmPinR = 10;
int enPin = 7;

float angle = 0;
float angle_old = 0;
float angle_dydx = 0;
float angle_integral = 0;
float balancetorque = 0;
float rest_angle = 0;
float currentspeed = 0;
int steeringZero = 0;
int steering = 0;
int steeringTemp = 0;

float p = 8; //2
float i = 0; //0.005
float d = 1300; //1000

float gyro_integration = 0;
float xZero = 0;
int gZero = 445; //this is always fixed, hence why no initialisation routine
unsigned long time, oldtime;
int pwmL;
int pwmR;
boolean over_angle = 0;



void setup() {
unsigned int i = 0;
unsigned long j = 0; //maximum possible value of j in routine is 102300 (100*1023)

pinMode(ledPin, OUTPUT); // declare the ledPin as an OUTPUT
Serial.begin(115200);
analogReference(EXTERNAL);
//----------------------------------------------------
TCCR1B = TCCR1B & 0b11111000 | 0x01;
analogWrite(pwmPinL,127);
analogWrite(pwmPinR,127);
digitalWrite(enPin,HIGH);
pinMode(enPin,OUTPUT);
digitalWrite(enPin,LOW);
//-----------------------------------------------------
delay(100);
for (i = 0; i < j =" j" steeringzero =" analogRead(steerPin);" xzero =" j/100;" oldtime =" micros();" time =" micros();">= (oldtime+5000)){
oldtime = time;
calculateAngle();

steering = (analogRead(steerPin) - steeringZero)/(15+(abs(angle)*8));

//-----OVER ANGLE PROTECTION-----
if (angle > 20 || angle < -20) { digitalWrite(enPin,HIGH); over_angle = 1; delay(500); } //-----END----- if (over_angle) { //if over_angle happened, give it a chance to reset when segway is level if (angle <> -1) {
digitalWrite(enPin, LOW);
over_angle = 0;
}
}
else {

//-----calculate rest angle-----
if (currentspeed > 10)
{
rest_angle = 0;
//-----END-----
angle_integral += angle;
balancetorque = ((angle+rest_angle)*p) + (angle_integral*i) + (angle_dydx*d);
angle_dydx = (angle - angle_old)/200; //now in degrees per second
angle_old = angle;
currentspeed += (balancetorque/200);

pwmL = (127 + balancetorque + steering);

//-----COERCE-----
if (pwmL < pwml =" 0;"> 255)
pwmL = 255;
//-----END-----

pwmR = (127 - balancetorque + steering);

//-----COERCE-----
if (pwmR < pwmr =" 0;"> 255)
pwmR = 255;
//-----END-----

analogWrite(pwmPinL, pwmL);
analogWrite(pwmPinR, pwmR);
}
}
}

void calculateAngle() {
//Analogref could be as small as 2.2V to improve step accuracy by ~30%
//uses small angle approximation that sin x = x (in rads). maybe use arcsin x for more accuracy?
//analogref is off the gyro power supply voltage, and routine is calibrated for 3.3V. maybe run acc/gyro/ref off 1 3.3V regulator, an
//accurately measure that.
//routine runs at 200hz because gyro maximum response rate = 200hz
float acc_angle = 0;
float gyro_angle = 0;

acc_angle = (((analogRead(xPin)-xZero)/310.3030)*(-57.2958);
gyro_angle = ((analogRead(gyroPin) - gZero)*4.8099)/200;
gyro_integration = gyro_integration + gyro_angle; //integration of gyro and gyro angle calculation
angle = (gyro_integration * 0.99) + (acc_angle * 0.01); //complementary filter
gyro_integration = angle; //drift correction of gyro integration

}

21 comments:

  1. any chance to get you to post schematics.....?

    ReplyDelete
  2. There appears to be an issue in the syntax of the for loop which follows delay(100);

    ReplyDelete
  3. something mistyped in the code after delay(100);

    ReplyDelete
  4. This comment has been removed by the author.

    ReplyDelete
  5. Hey,

    You inspired me into wanting to build one myself. Any chance of putting up a post with any resources you used? Also is it possible to get a contact email to ask questions to help me get started?

    Thanks

    ReplyDelete
  6. Hey,

    I was just poking through the code and wondered, what does pin 7, the enPin, do? Is it an external enable or inside the program loop and I just missed it? Thanks, and great build! I hope mine is as stable as yours!

    ReplyDelete
  7. Hello! iam From Peru! iam in the Catolica University and myself and my group are building a segway! well... trying haha we had build our own H bridge controllers with power mosftes , also we had to do our chasis , its very simillar tu yours, also we had a gear box ( we are using a 1/2 hp motr from robotmarketplace) maybe i will post some pics later! write my email pls! cesar.sasaki@gmail.com!

    ReplyDelete
  8. Somehow, this code has been mangled, and won't compile. The loop() function is completely missing (kind of an important part of an arduino program), and I'm scratching a hole in my head trying to figure out what the quoted portions are supposed to be.

    I have a robot with a very similar setup and I wanted to try to adapt this code, but there are a few pieces (aside from those that aren't even valid code) that are a bit confusing. A circuit diagram to go with it would go a long way, or at least a description of which arduino pins are connected to what. For instance, I don't see anything that looks like a control for the direction of the motors.

    ReplyDelete
  9. I just figured it out, the quotes are where blogger saw a greater-than sign followed eventually by a less-than sign, thought that it was meant to be an html tag, and tried to turn each assignment operation into an attribute name and value, horribly mangling the code and apparently deleting some important parts of it. Perhaps if you post it between <code> and </code>, the parser will leave it alone. I've posted php that way on blogger before, and it came out alright.

    ReplyDelete
  10. Hey, I was just wondering if you could repost the code with the void loop() and whatever is deleted here. I tried playing around with the posted code in the Arduino IDE interface, but can't get it working.

    That, or if your really nice you could send it to my email? (That's vexxir(a)hotmail.com), thanks!

    ReplyDelete
  11. Yeah, I would like to see the cleaned up code too, I'm working on something similar for a robot.

    ReplyDelete
  12. can you post the right code please?

    ReplyDelete
  13. Very interested in the Final code, You could try and upload to Github or even the Arduino sketch file to a server. Would love to get a hold of this, I have been trying to work out a project like this for a while.

    John
    j.hobson AT westnet DOT com DOT au

    ReplyDelete
  14. Cmon bro at least give us the code in a notepad file or something. I really need it.

    ReplyDelete
  15. can u upload the wiring setup??
    i found a mistake at the second IF HERE:
    //-----OVER ANGLE PROTECTION-----
    if (angle > 20 || angle < -20) { digitalWrite(enPin,HIGH); over_angle = 1; delay(500); } //-----END----- if (over_angle) { //if over_angle happened, give it a chance to reset when segway is level if (angle <> -1) {
    digitalWrite(enPin, LOW);
    over_angle = 0;
    }
    }
    else {

    ReplyDelete
  16. Id love to see a wiring diagram, this is really nice

    ReplyDelete
  17. Hello We could exchange some idea by e-mail? Thank you.

    ReplyDelete
  18. my email: matheus.lodi@gmail.com thanks

    ReplyDelete
  19. Hi Macaba, Can you email me the source code please?
    I would like to upgrade my segway to the Arduino platform.
    malcolm@faed.name. See http://members.optusnet.com.au/a4x4kiwi/scooter/ for mine.

    ReplyDelete
  20. Hi Macaba, Can you please email me your source code to chaman.nasa@gmail.com . I am using Arduino Uno controller, Sabertooth Motor Driver, MY1016 Scooter motor, MPU6050.

    Thanks and Regards

    ReplyDelete
  21. we are offering and provide Segway's United Kingdom you can giving a gift surprise on your child birthday we give you in whole sale price you can visit and check more information Segways United Kingdom

    ReplyDelete