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

}

60 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. It may be that your sole purpose in life is simply to serve as a warning to others. ....................................................

    ReplyDelete
  13. Hello,這裡真是百看不厭的部落格 .........................................

    ReplyDelete
  14. ^^ 謝謝你的分享,祝你生活永遠多彩多姿!........................................

    ReplyDelete
  15. 我們唯一需要恐懼的事,是恐懼本身........................................

    ReplyDelete
  16. 一個人就像一個分數,他的實際才能是分子,他對自己的評價是分母。分母越大,則分數的價值越小。 ....................................................

    ReplyDelete
  17. 朝著既定的目標走,就不會迷失。........................................

    ReplyDelete
  18. 一時的錯誤不算什麼,錯而不改才是一生中永遠且最大的錯誤......................................................

    ReplyDelete
  19. can you post the right code please?

    ReplyDelete
  20. 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
  21. 你不能決定生命的長度,但你可以控制它的寬度..................................................

    ReplyDelete
  22. 從人生中拿走友誼,猶如從生活中移走陽光 ............................................................

    ReplyDelete
  23. 在莫非定律中有項笨蛋定律:「一個組織中的笨蛋,恆大於等於三分之二。」......................................................................

    ReplyDelete
  24. 文章是心情的反應~~祝妳天天寫的都是讓人開心的好文章哦!!............................................................

    ReplyDelete
  25. 加油!充實內函最重要!Beauty is but skin- deep...................................................................

    ReplyDelete
  26. 不會從失敗中找尋教訓的人,成功之路是遙遠的。.................................................

    ReplyDelete
  27. 這不過是滑一跤,並不是死掉而爬不起來了。..................................................

    ReplyDelete
  28. 一棵樹除非在春天開了花,否則難望在秋天結果。..................................................

    ReplyDelete
  29. 喜歡看大家的文章,每篇都是一個故事,都是一種心情~~祝大家開心愉快...............................................................

    ReplyDelete
  30. 在莫非定律中有項笨蛋定律:「一個組織中的笨蛋,恆大於等於三分之二。」..................................................

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

    ReplyDelete
  32. 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
  33. Id love to see a wiring diagram, this is really nice

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

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

    ReplyDelete
  36. 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
  37. 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
  38. Thanks for sharing this valuable post here I really appreciate with this and like it so much. Please keep it up in future I am waiting for new update here.
    Self Balancing Scooter

    ReplyDelete
  39. 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