API tools faq
paste
Advertisement
Guest User

IRwalker.ino

a guest
Jan 10th, 2013
376
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 14.17 KB | None | 0 0
raw download clone embed print report
  1. //walker start code
  2.  
  3. //uses servo.h included with arduino 1.0 package
  4. #include <Servo.h>
  5. #include <IRremote.h> //non standard IRremote library from: https://github.com/shirriff/Arduino-IRremote
  6.  
  7. //servo naming
  8. //Front or Back
  9. //Right or Left
  10. //Upper or Lower
  11.  
  12. //pins choosen here
  13. //in the event of conflicts in numbering, use these
  14. #define FRU 11
  15. #define FRL 12
  16. #define FLU 9
  17. #define FLL 10
  18.  
  19. #define BRU 4
  20. #define BRL 5
  21. #define BLU 6
  22. #define BLL 7
  23.  
  24. //actual names for direct control
  25. //if you direct control, record the positions too
  26. Servo FRUs; //pin D11
  27. Servo FRLs; //pin D12
  28. Servo FLUs; //pin D9
  29. Servo FLLs; //pin D10
  30. Servo BRUs; //pin 4
  31. Servo BRLs; //pin 5
  32. Servo BLUs; //pin 6
  33. Servo BLLs; //pin 7
  34.  
  35. //calibration parmaters
  36. //angle offset from each other for balancing
  37. const int FRUoff = 0-25;
  38. const int FRLoff = 0;
  39. const int FLUoff = 0-25;
  40. const int FLLoff = 0;
  41. const int BRUoff = 0+25;
  42. const int BRLoff = 0;
  43. const int BLUoff = 0+25;
  44. const int BLLoff = 0;
  45. //-1 for reversal, 1 for standard
  46. const int FRUrev = -1;
  47. const int FRLrev = -1;
  48. const int FLUrev = 1;
  49. const int FLLrev = 1;
  50. const int BRUrev = -1;
  51. const int BRLrev = 1;
  52. const int BLUrev = 1;
  53. const int BLLrev = -1;
  54.  
  55. //this changes the speed of the bot
  56. //Don't plan on this being the actual speed since all servos move concurrently to reach destination at the same time
  57. //Only uses integar math still
  58. float degreesPerStep = 4;
  59. int stepTime = 25;
  60.  
  61. //holds the servo positions for smoothing movement
  62. int servoPos[8]; //order [FRU, FRL, FLU, FLL, BRU, BRL, BLU, BLL]
  63.  
  64. //commands go in this
  65. //TODO list commands
  66. //main command: ['g',FRU, FRL, FLU, FLL, BRU, BRL, BLU, BLL] sets all servos to the positions specified
  67. //ints passed as chars over serial, offset by 90 for unsigned
  68. int inByte = 0; // incoming serial byte default
  69.  
  70.  
  71. //IR stuff
  72. int RECV_PIN = 2; //IR decoder on pin D2!!
  73. IRrecv irrecv(RECV_PIN);
  74. decode_results results;
  75.  
  76.  
  77. void setup()
  78. {
  79. FRUs.attach(FRU);
  80. FRLs.attach(FRL);
  81. FLUs.attach(FLU);
  82. FLLs.attach(FLL);
  83.  
  84. BRUs.attach(BRU);
  85. BRLs.attach(BRL);
  86. BLUs.attach(BLU);
  87. BLLs.attach(BLL);
  88.  
  89. //open serial comm
  90. Serial.begin(9600);
  91.  
  92. stand(); //put it in a known position
  93.  
  94. irrecv.enableIRIn(); // Start the IR receiver
  95.  
  96. Serial.println("Finished Startup");
  97. }
  98.  
  99. //takes name of servo and position and sets to that
  100. //approx -90 to 90 range
  101. //modified by calibration parameters
  102. //records the current servo position when modified
  103. int servoSet(int name, int pos)
  104. {
  105. switch (name){
  106. case FRU:
  107. FRUs.write(90+(pos+FRUoff)*FRUrev);
  108. servoPos[0] = pos;
  109. break;
  110. case FRL:
  111. FRLs.write(90+(pos+FRLoff)*FRLrev);
  112. servoPos[1] = pos;
  113. break;
  114. case FLU:
  115. FLUs.write(90+(pos+FLUoff)*FLUrev);
  116. servoPos[2] = pos;
  117. break;
  118. case FLL:
  119. FLLs.write(90+(pos+FLLoff)*FLLrev);
  120. servoPos[3] = pos;
  121. break;
  122. case BRU:
  123. BRUs.write(90+(pos+BRUoff)*BRUrev);
  124. servoPos[4] = pos;
  125. break;
  126. case BRL:
  127. BRLs.write(90+(pos+BRLoff)*BRLrev);
  128. servoPos[5] = pos;
  129. break;
  130. case BLU:
  131. BLUs.write(90+(pos+BLUoff)*BLUrev);
  132. servoPos[6] = pos;
  133. break;
  134. case BLL:
  135. BLLs.write(90+(pos+BLLoff)*BLLrev);
  136. servoPos[7] = pos;
  137. break;
  138. default:
  139. return 0;
  140. break;
  141. }
  142. return 1;
  143. }
  144.  
  145. //IR decoding
  146. //These are captured codes from my IR transmittrer
  147. //hexbug spider channel A
  148. //this will output unrecognized codes over serial to capture your own if needed
  149. //hold button and disregard all values that aren't repearing
  150. //we're not aiming for perfect capture so some dropping is fine
  151. char getIR(){
  152. char result = '0';
  153. if (irrecv.decode(&results)) {
  154. int value = results.value;
  155. switch (value){
  156. case 0xED5A0B2B:
  157. Serial.println("forward");
  158. result = 'F';
  159. break;
  160. case 0x324FBF0A:
  161. Serial.println("back");
  162. result = 'B';
  163. break;
  164. case 0x23B42182:
  165. Serial.println("right");
  166. result = 'R';
  167. break;
  168. case 0x4AAFE047:
  169. Serial.println("left");
  170. result = 'L';
  171. break;
  172. default:
  173. Serial.print("Bad code: ");
  174. Serial.println(results.value, HEX);
  175. break;
  176. }
  177. irrecv.resume(); // Receive the next value
  178. }
  179. return result;
  180. }
  181.  
  182. //These step commands are copy overs from my python test script
  183. int forwardStep(){
  184. Serial.println("I'll try my best....");
  185. int stepAngle = -30; //this is the only change over walk, changes the direction of swivel
  186. int liftAngle = 25;
  187. int stepPos[8] = {0,40,0,40,0,40,0,40}; //start position
  188.  
  189. goTo(stepPos, 10, 30);
  190. stepPos[1] = stepPos[1]-liftAngle; //lift FR
  191. stepPos[7] = stepPos[7]-liftAngle; //lift BL
  192. goTo(stepPos, 10, 30);
  193.  
  194. stepPos[0] = stepPos[0]+stepAngle; //swivel FR forward
  195. stepPos[6] = stepPos[6]+stepAngle; //swivel BL forward
  196. stepPos[2] = stepPos[2]-stepAngle; //swivel FL back
  197. stepPos[4] = stepPos[4]-stepAngle;// swivel BR back
  198. goTo(stepPos, 10, 30);
  199.  
  200. stepPos[1] = stepPos[1]+liftAngle; //drop FR
  201. stepPos[7] = stepPos[7]+liftAngle;// #drop BL
  202. stepPos[3] = stepPos[3]-liftAngle;// #lift FL
  203. stepPos[5] = stepPos[5]-liftAngle;// #lift BR
  204. goTo(stepPos, 10, 30);
  205.  
  206. stepPos[0] = stepPos[0]-2*stepAngle;// #swivel FR back x2
  207. stepPos[6] = stepPos[6]-2*stepAngle;// #swivel BL back x2
  208. stepPos[2] = stepPos[2]+2*stepAngle;// #swivel FL forward x2
  209. stepPos[4] = stepPos[4]+2*stepAngle;// #swivel BR forward x2
  210. goTo(stepPos, 10, 30);
  211.  
  212. stepPos[3] = stepPos[3]+liftAngle;// #drop FL
  213. stepPos[5] = stepPos[5]+liftAngle;// #drop BR
  214. goTo(stepPos, 10, 30);
  215.  
  216. }
  217. int backwardStep(){
  218. Serial.println("I'll try my best....");
  219. int stepAngle = 30; //this is the only change over walk, changes the direction of swivel
  220. int liftAngle = 25;
  221. int stepPos[8] = {0,40,0,40,0,40,0,40}; //start position
  222.  
  223. goTo(stepPos, 10, 30);
  224. stepPos[1] = stepPos[1]-liftAngle; //lift FR
  225. stepPos[7] = stepPos[7]-liftAngle; //lift BL
  226. goTo(stepPos, 10, 30);
  227.  
  228. stepPos[0] = stepPos[0]+stepAngle; //swivel FR forward
  229. stepPos[6] = stepPos[6]+stepAngle; //swivel BL forward
  230. stepPos[2] = stepPos[2]-stepAngle; //swivel FL back
  231. stepPos[4] = stepPos[4]-stepAngle;// swivel BR back
  232. goTo(stepPos, 10, 30);
  233.  
  234. stepPos[1] = stepPos[1]+liftAngle; //drop FR
  235. stepPos[7] = stepPos[7]+liftAngle;// #drop BL
  236. stepPos[3] = stepPos[3]-liftAngle;// #lift FL
  237. stepPos[5] = stepPos[5]-liftAngle;// #lift BR
  238. goTo(stepPos, 10, 30);
  239.  
  240. stepPos[0] = stepPos[0]-2*stepAngle;// #swivel FR back x2
  241. stepPos[6] = stepPos[6]-2*stepAngle;// #swivel BL back x2
  242. stepPos[2] = stepPos[2]+2*stepAngle;// #swivel FL forward x2
  243. stepPos[4] = stepPos[4]+2*stepAngle;// #swivel BR forward x2
  244. goTo(stepPos, 10, 30);
  245.  
  246. stepPos[3] = stepPos[3]+liftAngle;// #drop FL
  247. stepPos[5] = stepPos[5]+liftAngle;// #drop BR
  248. goTo(stepPos, 10, 30);
  249. }
  250. int turnRight(){
  251. int stepAngle = -30; //only changed here
  252. int liftAngle = 25;
  253. int stepPos[8] = {0,40,0,40,0,40,0,40};// #goto stand
  254. goTo(stepPos, 10, 30);
  255.  
  256. stepPos[3] = stepPos[3]-liftAngle;// #lift FL
  257. stepPos[5] = stepPos[5]-liftAngle;// #lift BR
  258. goTo(stepPos, 10, 30);
  259.  
  260. stepPos[2] = stepPos[2]+stepAngle;// #swivel FL forward
  261. stepPos[6] = stepPos[6]-stepAngle;// #swivel BL back
  262.  
  263. stepPos[4] = stepPos[4]-stepAngle;// #swivel BR back
  264. stepPos[0] = stepPos[0]+stepAngle;// #swivel FR forward
  265. goTo(stepPos, 10, 30);
  266.  
  267. stepPos[3] = stepPos[3]+liftAngle;// #drop FL
  268. stepPos[7] = stepPos[7]-liftAngle;// #lift BL
  269. stepPos[5] = stepPos[5]+liftAngle;// #drop BR
  270. stepPos[1] = stepPos[1]-liftAngle;// #lift FR
  271. goTo(stepPos, 10, 30);
  272.  
  273. stepPos[2] = stepPos[2]-stepAngle;// #swivel FL back //oldx2
  274. stepPos[6] = stepPos[6]+stepAngle;// #swivel BL forward //oldx2
  275. stepPos[4] = stepPos[4]+stepAngle;// #swivel BR forward //oldx2
  276. stepPos[0] = stepPos[0]-stepAngle;// #swivel FR back //oldx2
  277. goTo(stepPos, 10, 30);
  278.  
  279. stepPos[7] = stepPos[7]+liftAngle;// #drop BL
  280. stepPos[1] = stepPos[1]+liftAngle;// #drop FR
  281. goTo(stepPos, 10, 30);
  282. }
  283. int turnLeft(){
  284. int stepAngle = 30; //only changed here
  285. int liftAngle = 25;
  286. int stepPos[8] = {0,40,0,40,0,40,0,40};// #goto stand
  287. goTo(stepPos, 10, 30);
  288.  
  289. stepPos[3] = stepPos[3]-liftAngle;// #lift FL
  290. stepPos[5] = stepPos[5]-liftAngle;// #lift BR
  291. goTo(stepPos, 10, 30);
  292.  
  293. stepPos[2] = stepPos[2]+stepAngle;// #swivel FL forward
  294. stepPos[6] = stepPos[6]-stepAngle;// #swivel BL back
  295.  
  296. stepPos[4] = stepPos[4]-stepAngle;// #swivel BR back
  297. stepPos[0] = stepPos[0]+stepAngle;// #swivel FR forward
  298. goTo(stepPos, 10, 30);
  299.  
  300. stepPos[3] = stepPos[3]+liftAngle;// #drop FL
  301. stepPos[7] = stepPos[7]-liftAngle;// #lift BL
  302. stepPos[5] = stepPos[5]+liftAngle;// #drop BR
  303. stepPos[1] = stepPos[1]-liftAngle;// #lift FR
  304. goTo(stepPos, 10, 30);
  305.  
  306. stepPos[2] = stepPos[2]-stepAngle;// #swivel FL back //oldx2
  307. stepPos[6] = stepPos[6]+stepAngle;// #swivel BL forward //oldx2
  308. stepPos[4] = stepPos[4]+stepAngle;// #swivel BR forward //oldx2
  309. stepPos[0] = stepPos[0]-stepAngle;// #swivel FR back //oldx2
  310. goTo(stepPos, 10, 30);
  311.  
  312. stepPos[7] = stepPos[7]+liftAngle;// #drop BL
  313. stepPos[1] = stepPos[1]+liftAngle;// #drop FR
  314. goTo(stepPos, 10, 30);
  315. }
  316.  
  317. //listens to the serial port for a array of locations and then sets the servos to that postion
  318. //this uses a default speed that may need to be changed
  319. //note that there is a 90 degree offset in the angle as sent over serial
  320. int go(){
  321. delay(100); //wait to make sure the rest of the command arrives
  322. int readDest[8];
  323. if (Serial.available() >= 8) { //read the array
  324. for (int i = 0; i<8; i++){
  325. readDest[i]=Serial.read()-90; //90 degree shift since unsigned over serial
  326. }
  327. }
  328. else{
  329. Serial.println("Go command FUBAR -- NO DEAL!");
  330. return 0;
  331. }
  332. //space reserved for sanity checking...
  333. goTo(readDest, 10, 30);
  334. }
  335.  
  336. //this was the test command
  337. //depreciated
  338. int flex(){
  339. for (int i = 15-45; i < 15+45; i+=5){
  340. int stepPos[8] = {i,40,i,40,i,40,i,40}; //start position
  341. goTo(stepPos, 10, 30);
  342. }
  343. }
  344.  
  345. //reports the current servo positions over the serial
  346. int reportPos(){
  347. Serial.println("Current positions as follows");
  348. for (int i = 0; i < 8; i++){
  349. Serial.println(servoPos[i]);
  350. }
  351. }
  352.  
  353. //idiot proof error state condition
  354. //snaps direct to position
  355. //--may be rough on the bot
  356. int standOld(){
  357. servoSet(FLU, 0);
  358. servoSet(FRU, 0);
  359. servoSet(FLL, 40);
  360. servoSet(FRL, 40);
  361. servoSet(BLU, 0);
  362. servoSet(BRU, 0);
  363. servoSet(BLL, 40);
  364. servoSet(BRL, 40);
  365. }
  366.  
  367. //goes to a desired position in a smooth action
  368. //disregards the legacy parameters -> users global variables and calculates
  369. int goTo(int desired[], int stepNumOld, int waitTimeOld){
  370. //we will now be ignoring the legacy stepNum and calculating our own
  371. int maxDelta = 0;
  372. for (int i = 0; i<8; i++){
  373. if (abs(servoPos[i]-desired[i]) >= maxDelta){
  374. maxDelta = abs(servoPos[i]-desired[i]);
  375. }
  376. }
  377. //disregard the old waitTime
  378. int waitTime = stepTime;
  379. int stepNum = maxDelta/degreesPerStep;
  380. //calculate step size
  381. int steps[8];
  382. steps[0] = (servoPos[0]-(desired[0]))/-stepNum;
  383. steps[1] = (servoPos[1]-(desired[1]))/-stepNum;
  384. steps[2] = (servoPos[2]-(desired[2]))/-stepNum;
  385. steps[3] = (servoPos[3]-(desired[3]))/-stepNum;
  386. steps[4] = (servoPos[4]-(desired[4]))/-stepNum;
  387. steps[5] = (servoPos[5]-(desired[5]))/-stepNum;
  388. steps[6] = (servoPos[6]-(desired[6]))/-stepNum;
  389. steps[7] = (servoPos[7]-(desired[7]))/-stepNum;
  390.  
  391. //do the stepping
  392. for (int j = 0; j < stepNum; j++){
  393. servoSet(FRU, servoPos[0]+steps[0]);
  394. servoSet(FRL, servoPos[1]+steps[1]);
  395. servoSet(FLU, servoPos[2]+steps[2]);
  396. servoSet(FLL, servoPos[3]+steps[3]);
  397. servoSet(BRU, servoPos[4]+steps[4]);
  398. servoSet(BRL, servoPos[5]+steps[5]);
  399. servoSet(BLU, servoPos[6]+steps[6]);
  400. servoSet(BLL, servoPos[7]+steps[7]);
  401. delay(waitTime);
  402. }
  403.  
  404. //set direct to account for error
  405. servoSet(FRU, desired[0]);
  406. servoSet(FRL, desired[1]);
  407. servoSet(FLU, desired[2]);
  408. servoSet(FLL, desired[3]);
  409. servoSet(BRU, desired[4]);
  410. servoSet(BRL, desired[5]);
  411. servoSet(BLU, desired[6]);
  412. servoSet(BLL, desired[7]);
  413.  
  414. }
  415.  
  416. //puts the servos all in a safe standing position
  417. int stand(){
  418. int standPos[8] = {0,40,0,40,0,40,0,40};
  419. goTo(standPos, 10, 30);
  420. }
  421.  
  422. //sets the bot down on it's belly
  423. int down(){
  424. int downPos[8] = {0,-45,0,-45,0,-45,0,-45};
  425. goTo(downPos, 10, 30);
  426. }
  427.  
  428. //the ascii commands are converted to ints on the serial
  429. //comparison is done vs their equivalent number
  430. int takeCommand(int input){
  431. switch (input){
  432. case 115: //ascii 's' for stand
  433. Serial.println("standing up");
  434. stand();
  435. break;
  436. case 100: //ascii 'd' for sit Down
  437. Serial.println("belly flop");
  438. down();
  439. break;
  440. case 114: //ascii 'r' for report
  441. Serial.println("report");
  442. reportPos();
  443. break;
  444. case 101: //ascii 'e' for error state
  445. Serial.println("idiot proofing");
  446. standOld();
  447. break;
  448. case 102: //'f' for forward step
  449. forwardStep();
  450. break;
  451. case 116: //'t' for test pose
  452. flex();
  453. break;
  454. case 103: //'g' for go - it is anticiated this will be the main command
  455. go();
  456. break;
  457. //for the IR stuff
  458. //hard coded steps for remote control
  459. case 70:
  460. forwardStep();
  461. break;
  462. case 66:
  463. backwardStep();
  464. break;
  465. case 82:
  466. turnRight();
  467. break;
  468. case 76:
  469. turnLeft();
  470. break;
  471. default: //this will tell you the code of what you entered if you listen on the serial
  472. //Serial.print("I don't know what this means:");
  473. // Serial.println(input);
  474. return 0;
  475. }
  476. }
  477.  
  478. void loop()
  479. {
  480. //This uses IR control
  481. //takeCommand(getIR());
  482. //getIR();
  483.  
  484. // this uses serial control
  485. if (Serial.available() > 0) {
  486. // get incoming byte:
  487. inByte = Serial.read();
  488. takeCommand(inByte);
  489. Serial.println("got it"); //an ack that includes a 'g' is needed for the simple interface
  490. }
  491. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!