1. /segbotler self balancing robot 2013
  2. //Austin Dovel
  3.  
  4. #include
  5. int x, r, l, s, f, xa, xb, xc;
  6.  
  7. //Digital pin 13 is serial transmit pin to sabertooth
  8. #define SABER_TX_PIN 13
  9. //Not used but still initialised, Digital pin 12 is serial receive from Sabertooth
  10. #define SABER_RX_PIN 12
  11. //set baudrate to match sabertooth dip settings
  12. #define SABER_BAUDRATE 9600
  13. SoftwareSerial SaberSerial = SoftwareSerial (SABER_RX_PIN, SABER_TX_PIN );
  14.  
  15. void initSabertooth (void) {
  16. //communicate with sabertooth
  17. pinMode ( SABER_TX_PIN, OUTPUT );
  18. SaberSerial.begin( SABER_BAUDRATE );
  19. }
  20.  
  21. void setup() // run once, when the sketch starts
  22. {
  23. initSabertooth();
  24. //analogINPUTS
  25. Serial.begin(9600);
  26. }
  27.  
  28. void set_motor() {
  29. x = analogRead(0); // read analog input pin 0
  30.  
  31. //x range is about 270-400 and flat is about 330
  32.  
  33. //smooth x by averaging 3 readings of x
  34. xa = x;
  35. delay (20);
  36. x = analogRead(0);
  37. xb = x;
  38. delay (20);
  39. x = analogRead(0);
  40. xc = x;
  41. x= (xa +xb + xc)/3;
  42.  
  43.  
  44. //SABER_right_FULL_FORWARD 127
  45. //SABER_right_FULL_REVERSE 1
  46. //SABER_left_FULL_FORWARD 255
  47. //SABER_left_FULL_REVERSE 128
  48.  
  49. //s=slope with less being more aggressive
  50. s = 1.8 ;
  51. //f=fudge factor
  52. f = 5;
  53.  
  54.  
  55. //stable x around 330
  56. if ((x > 325) && (x < 335)) {
  57. r = 62;
  58. l = 194;
  59. }
  60. //drive forward at a steady speed if leaning forward a little 310 > x < 330
  61. if ((x > 310) && (x < 326)) {
  62. r = 45;
  63. l = 167;
  64. }
  65. //if falling forward more increase speed linearly for 279 > x < 311
  66. if ((x > 279) && (x < 311)) {
  67. //higher values make it faster
  68. r = s * x - 278 + f;
  69. l = s * x - 148 + f;
  70. }
  71. //if full forward x < 280
  72. if ((x > 250) && (x < 280)) {
  73. r = 6;
  74. l = 133;
  75. }
  76. // drive backward at a steady speed if leaning back a little 334 > x > 349
  77. if ((x > 334) && (x < 349)) {
  78. r = 78;
  79. l = 208;
  80. }
  81. //if falling backwords more increase speed linearly for 348 < x < 390
  82. if ((x > 348) && (x < 391)) {
  83. //lower values make it faster
  84. r = s * x - 270 + f;
  85. l = s * x - 140 + f;
  86. }
  87. //if full backwords 390 < x
  88. if ((x > 390) && (x < 410)) {
  89. r = 122;
  90. l = 250;
  91. }
  92.  
  93. //send motor outputs to sabertooth
  94. SaberSerial.write(byte(r));
  95. SaberSerial.write(byte(l));
  96.  
  97. }
  98.  
  99. void loop () {
  100.  
  101. float level = 0;
  102.  
  103. int u;
  104.  
  105. set_motor();
  106.  
  107. } // end loop