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

}

57 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. ^^ 謝謝你的分享,祝你生活永遠多彩多姿!........................................

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

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

    ReplyDelete
  16. can you post the right code please?

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

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

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

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

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

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

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

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

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

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

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