Advertisement
mericaa

Wild Thumper beta code

Sep 27th, 2013
230
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <Servo.h>
  2. #include "IOpins.h"
  3. #include "Constants.h"
  4.  
  5.  
  6. //-------------------------------------------------------------- define global variables ---------------------
  7.  
  8. unsigned int Volts;
  9. unsigned int LeftAmps;
  10. unsigned int RightAmps;
  11. unsigned long chargeTimer;
  12. unsigned long leftoverload;
  13. unsigned long rightoverload;
  14. int highVolts;
  15. int startVolts;
  16. int Leftspeed=0;
  17. int Rightspeed=0;
  18. int Speed;
  19. int Steer;
  20. byte Charged=1; // 0=Flat battery 1=Charged battery
  21. int Leftmode=1; // 0=reverse, 1=brake, 2=forward
  22. int Rightmode=1; // 0=reverse, 1=brake, 2=forward
  23. byte Leftmodechange=0; // Left input must be 1500 before brake or reverse can occur
  24. byte Rightmodechange=0; // Right input must be 1500 before brake or reverse can occur
  25. int LeftPWM; // PWM value for left motor speed / brake
  26. int RightPWM; // PWM value for right motor speed / brake
  27. int data;
  28. int servo[7];
  29. const int trigger = 2; // aaa Define Ping Trigger as D2
  30. const int echo = 4; // aaa Define Ping Echo D4
  31.  
  32. //-------------------------------------------------------------- define servos --------------------------------
  33.  
  34.  
  35. Servo Servo0; // define servos
  36. Servo Servo1; // define servos
  37. Servo Servo2; // define servos
  38. Servo Servo3; // define servos
  39. Servo Servo4; // define servos
  40. Servo Servo5; // define servos
  41. Servo Servo6; // define servos
  42.  
  43. void setup()
  44. {
  45. //------------------------------------------------------------ Initialize Servos ------------------------------
  46.  
  47. //Servo0.attach(S0); // attach servo to I/O pin
  48. //Servo1.attach(S1); // attach servo to I/O pin
  49. Servo2.attach(S2); // attach servo to I/O pin
  50. Servo3.attach(S3); // attach servo to I/O pin
  51. Servo4.attach(S4); // attach servo to I/O pin
  52. Servo5.attach(S5); // attach servo to I/O pin
  53. Servo6.attach(S6); // attach servo to I/O pin
  54.  
  55. //------------------------------------------------------------ Set servos to default position -----------------
  56.  
  57. Servo0.writeMicroseconds(DServo0); // set servo to default position
  58. Servo1.writeMicroseconds(DServo1); // set servo to default position
  59. Servo2.writeMicroseconds(DServo2); // set servo to default position
  60. Servo3.writeMicroseconds(DServo3); // set servo to default position
  61. Servo4.writeMicroseconds(DServo4); // set servo to default position
  62. Servo5.writeMicroseconds(DServo5); // set servo to default position
  63. Servo6.writeMicroseconds(DServo6); // set servo to default position
  64.  
  65. //------------------------------------------------------------ Initialize I/O pins ---------------------------
  66.  
  67. pinMode (Charger,OUTPUT); // change Charger pin to output
  68. digitalWrite (Charger,1); // disable current regulator to charge battery
  69.  
  70. if (Cmode==1)
  71. {
  72. Serial.begin(Brate); // enable serial communications if Cmode=1
  73. Serial.flush(); // flush buffer
  74. }
  75. //Serial.begin(57600);
  76.  
  77. //------------------------------------------------------------ Initilialize I/O Ping Sensor -----------------
  78.  
  79. pinMode(trigger,OUTPUT); // aaa Set digital pin 2 to trigger output
  80. pinMode(echo,INPUT); // aaa Set digital pin 4 to echo input
  81. }
  82.  
  83.  
  84. void loop()
  85. {
  86. //---------------------------------------- Check battery voltage and current draw of motors ---------------
  87.  
  88. Volts=analogRead(Battery); // read the battery voltage
  89. LeftAmps=analogRead(LmotorC); // read left motor current draw
  90. RightAmps=analogRead(RmotorC); // read right motor current draw
  91.  
  92. //Serial.print(LeftAmps);
  93. //Serial.print(" ");
  94. //Serial.println(RightAmps);
  95.  
  96. if (LeftAmps>Leftmaxamps) // is motor current draw exceeding safe limit
  97. {
  98. analogWrite (LmotorA,0); // turn off motors
  99. analogWrite (LmotorB,0); // turn off motors
  100. leftoverload=millis(); // record time of overload
  101. }
  102.  
  103. if (RightAmps>Rightmaxamps) // is motor current draw exceeding safe limit
  104. {
  105. analogWrite (RmotorA,0); // turn off motors
  106. analogWrite (RmotorB,0); // turn off motors
  107. rightoverload=millis(); // record time of overload
  108. }
  109.  
  110. if ((Volts<lowvolt) && (Charged==1)) // check condition of the battery
  111. { // change battery status from charged to flat
  112.  
  113. //-------------------------- FLAT BATTERY speed controller shuts down until battery is recharged ----
  114. //--------------------------- This is a safety feature to prevent malfunction at low voltages!! ------
  115.  
  116. Charged=0; // battery is flat
  117. highVolts=Volts; // record the voltage
  118. startVolts=Volts;
  119. chargeTimer=millis(); // record the time
  120.  
  121. digitalWrite (Charger,0); // enable current regulator to charge battery
  122. }
  123.  
  124. //------------------------------- CHARGE BATTERY -------------------------------------------------------
  125.  
  126. if ((Charged==0) && (Volts-startVolts>67)) // if battery is flat and charger has been connected (voltage has increased by at least 1V)
  127. {
  128. if (Volts>highVolts) // has battery voltage increased?
  129. {
  130. highVolts=Volts; // record the highest voltage. Used to detect peak charging.
  131. chargeTimer=millis(); // when voltage increases record the time
  132. }
  133.  
  134. if (Volts>batvolt) // battery voltage must be higher than this before peak charging can occur.
  135. {
  136. if ((highVolts-Volts)>5 || (millis()-chargeTimer)>chargetimeout) //has voltage begun to drop or levelled out?
  137. {
  138. Charged=1; // battery voltage has peaked
  139. digitalWrite (Charger,1); // turn off current regulator
  140. }
  141. }
  142. }
  143.  
  144. else
  145.  
  146. {//---------------------- GOOD BATTERY speed controller opperates normally ----------------------
  147.  
  148. switch(Cmode)
  149. {
  150. case 0: // RC mode via D0 and D1
  151. RCmode();
  152. break;
  153.  
  154. case 1: // Serial mode via D0(RX) and D1(TX)
  155. SCmode();
  156. break;
  157.  
  158. case 2: // I2C mode via A4(SDA) and A5(SCL)
  159. I2Cmode();
  160. break;
  161. }
  162.  
  163. // ------------------------------ Code to drive dual "H" bridges --------------------------------------
  164.  
  165. if (Charged==1) // Only power motors if battery voltage is good
  166. {
  167. if ((millis()-leftoverload)>overloadtime)
  168. {
  169. switch (Leftmode) // if left motor has not overloaded recently
  170. {
  171. case 2: // left motor forward (2 = forward)
  172. analogWrite(LmotorA,0);
  173. analogWrite(LmotorB,LeftPWM);
  174. break;
  175.  
  176. case 1: // left motor brake (1 = brake)
  177. analogWrite(LmotorA,LeftPWM);
  178. analogWrite(LmotorB,LeftPWM);
  179. break;
  180.  
  181. case 0: // left motor reverse (0 = reverse)
  182. analogWrite(LmotorA,LeftPWM);
  183. analogWrite(LmotorB,0);
  184. break;
  185. }
  186. }
  187. if ((millis()-rightoverload)>overloadtime)
  188. {
  189. switch (Rightmode) // if right motor has not overloaded recently
  190. {
  191. case 2: // right motor forward
  192. analogWrite(RmotorA,0);
  193. analogWrite(RmotorB,RightPWM);
  194. break;
  195.  
  196. case 1: // right motor brake
  197. analogWrite(RmotorA,RightPWM);
  198. analogWrite(RmotorB,RightPWM);
  199. break;
  200.  
  201. case 0: // right motor reverse
  202. analogWrite(RmotorA,RightPWM);
  203. analogWrite(RmotorB,0);
  204. break;
  205. }
  206. }
  207. }
  208. else // Battery is flat
  209. {
  210. analogWrite (LmotorA,0); // turn off motors
  211. analogWrite (LmotorB,0); // turn off motors
  212. analogWrite (RmotorA,0); // turn off motors
  213. analogWrite (RmotorB,0); // turn off motors
  214. }
  215. }
  216. }
  217.  
  218.  
  219.  
  220.  
  221.  
  222.  
  223. void RCmode()
  224. {
  225. //---------------------------- Code for RC inputs ---------------------------------------------------------
  226.  
  227. Speed=pulseIn(RCleft,HIGH,25000); // read throttle/left stick
  228. Steer=pulseIn(RCright,HIGH,25000); // read steering/right stick
  229.  
  230.  
  231. if (Speed==0) Speed=1500; // if pulseIn times out (25mS) then set speed to stop
  232. if (Steer==0) Steer=1500; // if pulseIn times out (25mS) then set steer to centre
  233.  
  234. if (abs(Speed-1500)<RCdeadband) Speed=1500; // if Speed input is within deadband set to 1500 (1500uS=center
  235. // position for most servos)
  236. if (abs(Steer-1500)<RCdeadband) Steer=1500; // if Steer input is within deadband set to 1500
  237. //(1500uS=center position for most servos)
  238.  
  239. if (Mix==1) // Mixes speed and steering signals
  240. {
  241. Steer=Steer-1500;
  242. Leftspeed=Speed-Steer;
  243. Rightspeed=Speed+Steer;
  244. }
  245. else // Individual stick control
  246. {
  247. Leftspeed=Speed;
  248. Rightspeed=Steer;
  249. }
  250. /*
  251. Serial.print("Left:");
  252. Serial.print(Leftspeed);
  253. Serial.print(" -- Right:");
  254. Serial.println(Rightspeed);
  255. */
  256. Leftmode=2;
  257. Rightmode=2;
  258. if (Leftspeed>(Leftcenter+RCdeadband)) Leftmode=0; // if left input is forward then set left mode to forward
  259. if (Rightspeed>(Rightcenter+RCdeadband)) Rightmode=0; // if right input is forward then set right mode to forward
  260.  
  261. LeftPWM=abs(Leftspeed-Leftcenter)*10/scale; // scale 1000-2000uS to 0-255
  262. LeftPWM=min(LeftPWM,255); // set maximum limit 255
  263.  
  264. RightPWM=abs(Rightspeed-Rightcenter)*10/scale; // scale 1000-2000uS to 0-255
  265. RightPWM=min(RightPWM,255); // set maximum limit 255
  266. }
  267.  
  268.  
  269.  
  270.  
  271.  
  272.  
  273.  
  274. void SCmode()
  275. {// ------------------------------------------------------------ Code for Serial Communications --------------------------------------
  276.  
  277. // FL = flush serial buffer
  278.  
  279. // AN = report Analog inputs 1-5
  280.  
  281. // SV = next 7 integers will be position information for servos 0-6
  282.  
  283. // HB = "H" bridge data - next 4 bytes will be:
  284. // left motor mode 0-2
  285. // left motor PWM 0-255
  286. // right motor mode 0-2
  287. // right motor PWM 0-255
  288.  
  289.  
  290. if (Serial.available()>1) // command available
  291. {
  292. int A=Serial.read();
  293. int B=Serial.read();
  294. int command=A*256+B;
  295. switch (command)
  296. {
  297. case 17996: // FL
  298. Serial.flush(); // flush buffer
  299. break;
  300.  
  301. case 16718: // AN - return values of analog inputs 1-5
  302. for (int i=1;i<6;i++) // index analog inputs 1-5
  303. {
  304. data=analogRead(i); // read 10bit analog input
  305. Serial.write(highByte(data)); // transmit high byte
  306. Serial.write(lowByte(data)); // transmit low byte
  307. }
  308. break;
  309.  
  310. case 21334: // SV - receive postion information for servos 0-6
  311. for (int i=0;i<15;i++) // read 14 bytes of data
  312. {
  313. Serialread();
  314. servo[i]=data;
  315. }
  316. Servo0.writeMicroseconds(servo[0]*256+servo[1]); // set servo position
  317. Servo1.writeMicroseconds(servo[2]*256+servo[3]); // set servo position
  318. Servo2.writeMicroseconds(servo[4]*256+servo[5]); // set servo position
  319. Servo3.writeMicroseconds(servo[6]*256+servo[7]); // set servo position
  320. Servo4.writeMicroseconds(servo[8]*256+servo[9]); // set servo position
  321. Servo5.writeMicroseconds(servo[10]*256+servo[11]); // set servo position
  322. Servo6.writeMicroseconds(servo[12]*256+servo[13]); // set servo position
  323. break;
  324.  
  325. case 18498: // HB - mode and PWM data for left and right motors
  326. Serialread();
  327. Leftmode=data;
  328. Serialread();
  329. LeftPWM=data;
  330. Serialread();
  331. Rightmode=data;
  332. Serialread();
  333. RightPWM=data;
  334. break;
  335.  
  336. default: // invalid command
  337. Serial.flush(); // flush buffer
  338. }
  339. }
  340. }
  341.  
  342. void Serialread()
  343. {//------------------ Read serial port until data has been received -----------------------------------
  344. do
  345. {
  346. data=Serial.read();
  347. } while (data<0);
  348. }
  349.  
  350.  
  351.  
  352.  
  353.  
  354.  
  355.  
  356. void I2Cmode()
  357. {//----------------------- Your code goes here ------------------------------------------------------
  358.  
  359. void loop(){
  360.  
  361. RightPWM = 128; // sets the speed to half full speed
  362. LeftPWM = 128;
  363. Rightmode = 2; // sets the right motor to move forward (at half speed)
  364. Leftmode = 2; // see code to drive dual H bridges
  365.  
  366. unsigned long duration, cm;
  367.  
  368. digitalWrite(trigger, LOW); // Ensures a clean trigger pulse
  369. delayMicroseconds(5); // .1 ms long cleaning
  370. digitalWrite(trigger, HIGH); // Turn trigger pin on high (5V) to initiate pulse
  371. delayMicroseconds(10); // delay 10 us (minimum time to start pulse)
  372. digitalWrite(trigger, LOW); // stop tigger pulse to listen to echo
  373.  
  374. duration = pulseIn(echo, HIGH); // records the length of the echo pulse
  375. cm = duration/58; // converts duration into centimeters
  376.  
  377. if (cm < 100);{ // is there an obstacle < 100cm away?
  378. RightPWM = 256; // set right motor full speed
  379. LeftPWM = 0; // set left motor to 0, therefore turning left
  380. delay(3000); // allow 1s for turn to complete
  381. RightPWM = 0;
  382. LeftPWM = 256; // turn right (straighten back out)
  383. delay(3000);
  384. RightPWM = 128; // back to half speed forward
  385. LeftPWM = 128;
  386. }
  387. }
  388. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement