Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- /segbotler self balancing robot 2013
- //Austin Dovel
- #include
- int x, r, l, s, f, xa, xb, xc;
- //Digital pin 13 is serial transmit pin to sabertooth
- #define SABER_TX_PIN 13
- //Not used but still initialised, Digital pin 12 is serial receive from Sabertooth
- #define SABER_RX_PIN 12
- //set baudrate to match sabertooth dip settings
- #define SABER_BAUDRATE 9600
- SoftwareSerial SaberSerial = SoftwareSerial (SABER_RX_PIN, SABER_TX_PIN );
- void initSabertooth (void) {
- //communicate with sabertooth
- pinMode ( SABER_TX_PIN, OUTPUT );
- SaberSerial.begin( SABER_BAUDRATE );
- }
- void setup() // run once, when the sketch starts
- {
- initSabertooth();
- //analogINPUTS
- Serial.begin(9600);
- }
- void set_motor() {
- x = analogRead(0); // read analog input pin 0
- //x range is about 270-400 and flat is about 330
- //smooth x by averaging 3 readings of x
- xa = x;
- delay (20);
- x = analogRead(0);
- xb = x;
- delay (20);
- x = analogRead(0);
- xc = x;
- x= (xa +xb + xc)/3;
- //SABER_right_FULL_FORWARD 127
- //SABER_right_FULL_REVERSE 1
- //SABER_left_FULL_FORWARD 255
- //SABER_left_FULL_REVERSE 128
- //s=slope with less being more aggressive
- s = 1.8 ;
- //f=fudge factor
- f = 5;
- //stable x around 330
- if ((x > 325) && (x < 335)) {
- r = 62;
- l = 194;
- }
- //drive forward at a steady speed if leaning forward a little 310 > x < 330
- if ((x > 310) && (x < 326)) {
- r = 45;
- l = 167;
- }
- //if falling forward more increase speed linearly for 279 > x < 311
- if ((x > 279) && (x < 311)) {
- //higher values make it faster
- r = s * x - 278 + f;
- l = s * x - 148 + f;
- }
- //if full forward x < 280
- if ((x > 250) && (x < 280)) {
- r = 6;
- l = 133;
- }
- // drive backward at a steady speed if leaning back a little 334 > x > 349
- if ((x > 334) && (x < 349)) {
- r = 78;
- l = 208;
- }
- //if falling backwords more increase speed linearly for 348 < x < 390
- if ((x > 348) && (x < 391)) {
- //lower values make it faster
- r = s * x - 270 + f;
- l = s * x - 140 + f;
- }
- //if full backwords 390 < x
- if ((x > 390) && (x < 410)) {
- r = 122;
- l = 250;
- }
- //send motor outputs to sabertooth
- SaberSerial.write(byte(r));
- SaberSerial.write(byte(l));
- }
- void loop () {
- float level = 0;
- int u;
- set_motor();
- } // end loop
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement