1
0

mobileBase.ino 7.1 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318
  1. /*********************************************************************
  2. Description: A set of simple serial commands to control a omniWheel drive
  3. robot and receive back sensor and odometry data. Default configuration
  4. assumes use of an Arduino Mega 2560 + motor driver board. Edit the
  5. readEncoder() and setMotorSpeed() wrapper functions if using different
  6. motor controller or encoder method.
  7. Author: www.corvin.cn
  8. History: 20171129:init code;
  9. 20180209:增加代码版本号,定义初始版本号1.0,通过v命令来获取;
  10. 20180515:更新到1.1版本,整理代码架构,精简代码,使流程更清晰;
  11. *********************************************************************/
  12. #include "sound.h"
  13. #include "commands.h"
  14. #include "sensors.h"
  15. #include "motor_driver.h"
  16. #include "encoder_driver.h"
  17. #include "pid_controller.h"
  18. /******************** USER AREAR ********************/
  19. /* current code version */
  20. #define VERSION 1.1
  21. /* Stop the robot if it hasn't received a movement command
  22. in this number of milliseconds */
  23. #define AUTO_STOP_INTERVAL 260
  24. /******************** USER END **********************/
  25. /* Serial port baud rate */
  26. #define BAUDRATE 57600
  27. /* Run the PID loop at 30 times per second -Hz */
  28. #define PID_RATE 30
  29. /* Convert the rate into an interval */
  30. const int PID_INTERVAL = 1000 / PID_RATE;
  31. /* Track the next time we make a PID calculation */
  32. unsigned long nextPID = PID_INTERVAL;
  33. long lastMotorCommand = AUTO_STOP_INTERVAL;
  34. /* Variable initialization */
  35. // A pair of varibles to help parse serial commands (thanks Fergs)
  36. int arg = 0;
  37. int index = 0;
  38. // Variable to hold an input character
  39. char chr;
  40. // Variable to hold the current single-character command
  41. char cmd;
  42. // Character arrays to hold the first,second,third arguments
  43. char argv1[48];
  44. char argv2[5];
  45. char argv3[5];
  46. // The arguments converted to integers
  47. int arg1 = 0;
  48. int arg2 = 0;
  49. int arg3 = 0;
  50. /* Clear the current command parameters */
  51. void resetCommand()
  52. {
  53. cmd = '\0';
  54. memset(argv1, 0, sizeof(argv1));
  55. memset(argv2, 0, sizeof(argv2));
  56. memset(argv3, 0, sizeof(argv3));
  57. arg1 = 0;
  58. arg2 = 0;
  59. arg3 = 0;
  60. arg = 0;
  61. index = 0;
  62. }
  63. /* Run a command. Commands are defined in commands.h */
  64. int runCommand()
  65. {
  66. int i = 0;
  67. char *p = argv1; //p pointer for update pid parameter
  68. char *str;
  69. int pid_args[12];
  70. if (cmd != 'u') //cmd don't match update pid will convert
  71. {
  72. arg1 = atoi(argv1);
  73. arg2 = atoi(argv2);
  74. arg3 = atoi(argv3);
  75. }
  76. switch (cmd)
  77. {
  78. case GET_BAUDRATE: //'b'
  79. Serial.println(BAUDRATE);
  80. break;
  81. case ANALOG_READ: //'a'
  82. Serial.println(analogRead(arg1));
  83. break;
  84. case DIGITAL_READ: //'d'
  85. Serial.println(digitalRead(arg1));
  86. break;
  87. case ANALOG_WRITE:
  88. analogWrite(arg1, arg2);
  89. Serial.println("OK");
  90. break;
  91. case DIGITAL_WRITE: //'w'
  92. if (arg2 == 0)
  93. {
  94. digitalWrite(arg1, LOW);
  95. }
  96. else if (arg2 == 1)
  97. {
  98. digitalWrite(arg1, HIGH);
  99. }
  100. Serial.println("OK");
  101. break;
  102. case PIN_MODE:
  103. if (arg2 == 0)
  104. {
  105. pinMode(arg1, INPUT);
  106. }
  107. else if (arg2 == 1)
  108. {
  109. pinMode(arg1, OUTPUT);
  110. }
  111. Serial.println("OK");
  112. break;
  113. case READ_ENCODERS: //'e'
  114. Serial.print(readEncoder(A_WHEEL));
  115. Serial.print(" ");
  116. Serial.print(readEncoder(B_WHEEL));
  117. Serial.print(" ");
  118. Serial.println(readEncoder(C_WHEEL));
  119. break;
  120. case RESET_ENCODERS: //'r'
  121. resetEncoders();
  122. resetPID();
  123. Serial.println("OK");
  124. break;
  125. case MOTOR_SPEEDS: //'m'
  126. lastMotorCommand = millis(); /* Reset the auto stop timer */
  127. if (arg1 == 0 && arg2 == 0 && arg3 == 0)
  128. {
  129. setMotorSpeeds(0, 0, 0);
  130. resetPID();
  131. setMoveStatus(0);
  132. }
  133. else
  134. {
  135. setMoveStatus(1);
  136. }
  137. setWheelPIDTarget(arg1, arg2, arg3);
  138. Serial.println("OK");
  139. break;
  140. case UPDATE_PID: //'u'
  141. while ((str = strtok_r(p, ":", &p)) != '\0')
  142. {
  143. pid_args[i] = atoi(str);
  144. i++;
  145. }
  146. updatePIDParam(A_WHEEL, pid_args[0], pid_args[1], pid_args[2], pid_args[3]);
  147. updatePIDParam(B_WHEEL, pid_args[4], pid_args[5], pid_args[6], pid_args[7]);
  148. updatePIDParam(C_WHEEL, pid_args[8], pid_args[9], pid_args[10], pid_args[11]);
  149. Serial.println("OK");
  150. break;
  151. case SOUND_BEEP: //'f'
  152. if (arg1 == BASE_POWERON_BEEP)
  153. {
  154. basePowerOnBeep();
  155. }
  156. else if (arg1 == BASE_POWEROFF_BEEP)
  157. {
  158. basePowerOffBeep();
  159. }
  160. Serial.println("OK");
  161. break;
  162. case READ_PIDIN:
  163. Serial.print(readPidIn(A_WHEEL));
  164. Serial.print(" ");
  165. Serial.print(readPidIn(B_WHEEL));
  166. Serial.print(" ");
  167. Serial.println(readPidIn(C_WHEEL));
  168. break;
  169. case READ_PIDOUT:
  170. Serial.print(readPidOut(A_WHEEL));
  171. Serial.print(" ");
  172. Serial.print(readPidOut(B_WHEEL));
  173. Serial.print(" ");
  174. Serial.println(readPidOut(C_WHEEL));
  175. break;
  176. case CODE_VERSION:
  177. Serial.println(VERSION);
  178. break;
  179. default:
  180. Serial.println("Invalid Command");
  181. break;
  182. }
  183. return 0;
  184. }
  185. /* Setup function--runs once at startup. */
  186. void setup()
  187. {
  188. Serial.begin(BAUDRATE);
  189. initSensors();
  190. initSoundPin();
  191. initEncoders();
  192. initMotorsPinMode();
  193. resetPID();
  194. }
  195. /* Enter the main loop. Read and parse input from the serial port
  196. and run any valid commands. Run a PID calculation at the target
  197. interval and check for auto-stop conditions.*/
  198. void loop()
  199. {
  200. while (Serial.available() > 0)
  201. {
  202. chr = Serial.read(); //Read the next character
  203. if (chr == 13) //Terminate a command with a CR
  204. {
  205. if (arg == 1)
  206. {
  207. argv1[index] = '\0';
  208. }
  209. else if (arg == 2)
  210. {
  211. argv2[index] = '\0';
  212. }
  213. else if (arg == 3)
  214. {
  215. argv3[index] = '\0';
  216. }
  217. runCommand();
  218. resetCommand();
  219. }
  220. else if (chr == ' ') // Use spaces to delimit parts of the command
  221. {
  222. // Step through the arguments
  223. if (arg == 0)
  224. {
  225. arg = 1;
  226. }
  227. else if (arg == 1)
  228. {
  229. argv1[index] = '\0';
  230. arg = 2;
  231. index = 0;
  232. }
  233. else if (arg == 2)
  234. {
  235. argv2[index] = '\0';
  236. arg = 3;
  237. index = 0;
  238. }
  239. continue;
  240. }
  241. else // process single-letter command
  242. {
  243. if (arg == 0)
  244. {
  245. cmd = chr; //The first arg is the single-letter command
  246. }
  247. else if (arg == 1)
  248. {
  249. // Subsequent arguments can be more than one character
  250. argv1[index] = chr;
  251. index++;
  252. }
  253. else if (arg == 2)
  254. {
  255. argv2[index] = chr;
  256. index++;
  257. }
  258. else if (arg == 3)
  259. {
  260. argv3[index] = chr;
  261. index++;
  262. }
  263. }
  264. }//end while()
  265. //run a PID calculation at the appropriate intervals
  266. if (millis() > nextPID)
  267. {
  268. updatePID();
  269. nextPID += PID_INTERVAL;
  270. }
  271. //Check to see if we have exceeded the auto-stop interval
  272. if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL)
  273. {
  274. setMotorSpeeds(0, 0, 0); //stop motors
  275. setMoveStatus(0);
  276. }
  277. }