Advertisement
voradams

Arduino RC

Nov 4th, 2015
115
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 6.76 KB | None | 0 0
  1. // This code takes RC input from a standard PPM based RC receiver using separate steering and throttle channels
  2. // and mixes them for differential drive via a dual-motor h-bridge based on the L298N
  3. // Like this one http://www.amazon.com/DROK-Controller-H-Bridge-Mega2560-Duemilanove/dp/B00CAG6GX2/
  4.  
  5. // Pin definitions and control variables
  6. #define FORWARD 1
  7. #define BACKWARD 0
  8.  
  9. #define steeringInput 43
  10. #define throttleInput 42
  11.  
  12. #define motor1_enable 11
  13. #define motor1_pin1 2
  14. #define motor1_pin2 3
  15.  
  16. #define motor2_enable 3
  17. #define motor2_pin1 4
  18. #define motor2_pin2 5
  19.  
  20. #define LED_PIN 13
  21.  
  22. // Set this to 'true' in order to enable Serial debugging.
  23. bool debug = true;
  24. // disable outputting to motors (for Serial only debugging)
  25. bool motorsOff = true;
  26. // This will also introduce a 500ms delay between loops so as not to flood the console.
  27. bool delayDebug = true;
  28.  
  29.  
  30. // working variable declarations
  31. unsigned long steerRaw;
  32. unsigned long throttleRaw;
  33.  
  34. int throttleIn = 0;
  35. int steerIn = 0;
  36.  
  37. int motor1_output = 0;
  38. int motor2_output = 0;
  39. int motor1_dir = 0;
  40. int motor2_dir = 0;
  41.  
  42. int input_buffer = 20;
  43. int steerMin = 1350;
  44. int steerMax = 2600;
  45. int throttleMin = 1300;
  46. int throttleMax = 2600;
  47. int setBuffer = 600;
  48.  
  49. void setup()
  50. {
  51. bool error = false;
  52.  
  53. pinMode(steeringInput, INPUT);
  54. pinMode(throttleInput, INPUT);
  55.  
  56. pinMode(motor1_enable, OUTPUT);
  57. pinMode(motor1_pin1, OUTPUT);
  58. pinMode(motor1_pin2, OUTPUT);
  59. pinMode(motor2_pin1, OUTPUT);
  60. pinMode(motor2_pin2, OUTPUT);
  61. pinMode(motor2_enable, OUTPUT);
  62.  
  63. pinMode(LED_PIN, OUTPUT); // built in status LED
  64.  
  65. if(debug) {
  66. Serial.begin(9600);
  67. Serial.println("Arduino_RC");
  68. }
  69.  
  70. // Let's read in the average values and calculate a good center point.
  71. unsigned int throttleAvg = 0;
  72. unsigned int steerAvg = 0;
  73.  
  74. digitalWrite(LED_PIN, HIGH); // turn on the status LED while we're calibrating.
  75.  
  76. delay(2500);
  77.  
  78. for(int i=0;i<20;i++) {
  79. if(debug) {
  80. Serial.print("ThrottleAvg: ");
  81. Serial.println(throttleAvg);
  82. }
  83. throttleAvg += pulseIn(throttleInput, HIGH, 25000);
  84. steerAvg += pulseIn(steeringInput, HIGH, 25000);
  85. delay(2);
  86. }
  87.  
  88. if(debug) {
  89. Serial.print("throttleAvg (total): ");
  90. Serial.println(throttleAvg);
  91. Serial.print("steerAvg (total): ");
  92. Serial.println(steerAvg);
  93.  
  94. }
  95.  
  96. throttleAvg = (throttleAvg/20);
  97. steerAvg = (steerAvg/20);
  98.  
  99. // now that we have our averages, let's set the min/max values
  100.  
  101. if((steerAvg > 600) && (steerAvg < 2000)) {
  102. steerMin = steerAvg - setBuffer;
  103. steerMax = steerAvg + setBuffer;
  104. } else {
  105. error = true;
  106. }
  107. if((throttleAvg > 600) && (throttleAvg < 2000)) {
  108. throttleMin = throttleAvg - setBuffer;
  109. throttleMax = throttleAvg + setBuffer;
  110. } else {
  111. error = true;
  112. }
  113. if(error) {
  114. if(debug) {
  115. Serial.print("steerAvg: ");
  116. Serial.println(steerAvg);
  117. Serial.print("throttleAvg: ");
  118. Serial.println(throttleAvg);
  119. }
  120.  
  121. // trap the code and blink furiously
  122. while(true) {
  123. digitalWrite(LED_PIN, LOW);
  124. delay(50);
  125. digitalWrite(LED_PIN, HIGH);
  126. delay(50);
  127. }
  128. } else {
  129. if(debug) {
  130. Serial.print("Throttle: ");
  131. Serial.print(throttleMin);
  132. Serial.print(", ");
  133. Serial.println(throttleMax);
  134.  
  135. Serial.print("Steering: ");
  136. Serial.print(steerMin);
  137. Serial.print(", ");
  138. Serial.println(throttleMax);
  139. }
  140. }
  141.  
  142.  
  143. // turn off the LED
  144. digitalWrite(LED_PIN, LOW);
  145. delay(500);
  146.  
  147.  
  148. // do a happy dance
  149. for(int i=0;i<5;i++) {
  150. digitalWrite(LED_PIN, HIGH);
  151. delay(500);
  152. digitalWrite(LED_PIN, LOW);
  153. delay(100);
  154. }
  155.  
  156. } // end setup
  157.  
  158.  
  159. void getInputs() {
  160. steerRaw = pulseIn(steeringInput, HIGH);
  161. throttleRaw = pulseIn(throttleInput, HIGH);
  162.  
  163. if(steerRaw == 0 || throttleRaw == 0) {
  164. steerIn = 0;
  165. throttleIn = 0;
  166. }
  167. else {
  168. steerIn = constrain(map(steerRaw, steerMin, steerMax, -255, 255), -255, 255);
  169. throttleIn = constrain(map(throttleRaw, throttleMin, throttleMax, -255, 255), -255, 255);
  170. }
  171. } // end getInputs
  172.  
  173. void setDirection(int motor1_direction, int motor2_direction) {
  174.  
  175. if(motor1_direction == FORWARD) {
  176. digitalWrite(motor1_pin1, HIGH);
  177. digitalWrite(motor1_pin2, LOW);
  178. } else {
  179. digitalWrite(motor1_pin1, LOW);
  180. digitalWrite(motor1_pin2, HIGH);
  181. }
  182.  
  183.  
  184. if(motor2_direction == FORWARD) {
  185. digitalWrite(motor2_pin1, LOW);
  186. digitalWrite(motor2_pin2, HIGH);
  187. } else {
  188. digitalWrite(motor2_pin1, HIGH);
  189. digitalWrite(motor2_pin2, LOW);
  190. }
  191. } // end setDirection
  192.  
  193. void loop() {
  194. getInputs();
  195.  
  196. // Bias to going forward direction, this way spin-in-place works like you expect
  197. if(throttleIn >= -input_buffer) {
  198. motor1_output = throttleIn;
  199. motor2_output = throttleIn;
  200.  
  201. motor1_output -= steerIn;
  202. motor2_output += steerIn;
  203.  
  204. if(debug) {
  205. if(steerIn >= input_buffer) {
  206. // Steering is positive, so turn right
  207. Serial.println("Fwd + Right");
  208. } else if(steerIn <= -input_buffer) {
  209. // steerign is negative, so turn left
  210. Serial.println("Fwd + Left");
  211. } else {
  212. // this is our default state
  213. if(throttleIn == 0) {
  214. Serial.println("Stationary!");
  215. }
  216. else {
  217. Serial.println("Forwards!");
  218. }
  219. }
  220.  
  221. Serial.print("motor1_output: ");
  222. Serial.println(motor1_output);
  223. }
  224.  
  225. } else {
  226. // We allow negative values because we'll set direction and use abs to fix it before PWM'ing later.
  227. motor1_output = throttleIn;
  228. motor2_output = throttleIn;
  229.  
  230. motor1_output += steerIn;
  231. motor2_output -= steerIn;
  232.  
  233. if(debug) {
  234. if(steerIn >= input_buffer) {
  235. // steer backwards and to the right.
  236. Serial.println("Bk + Right");
  237. } else if(steerIn <= -input_buffer) {
  238. // steer backwards and to the left.
  239. Serial.println("Bk + Left");
  240. } else {
  241. // otherwise just go straight back
  242. Serial.println("Backwards!");
  243. }
  244.  
  245. Serial.print("motor1_output: ");
  246. Serial.println(motor1_output);
  247. }
  248.  
  249. }
  250.  
  251.  
  252. // Use the output values to determine direction.
  253. if(motor1_output < 0) {
  254. motor1_dir = BACKWARD;
  255. }
  256. else {
  257. motor1_dir = FORWARD;
  258. }
  259.  
  260. if(motor2_output < 0) {
  261. motor2_dir = BACKWARD;
  262. }
  263. else {
  264. motor2_dir = FORWARD;
  265. }
  266.  
  267. setDirection(motor1_dir, motor2_dir);
  268.  
  269. // PWM the enable pin to control the motor speed, being sure to only give a value from 0-255
  270. if(!motorsOff) {
  271. analogWrite(motor1_enable, abs(constrain(motor1_output, -255, 255)));
  272. analogWrite(motor2_enable, abs(constrain(motor2_output, -255, 255)));
  273. }
  274. if(delayDebug) {
  275. delay(500);
  276. }
  277. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement