Want more features on Pastebin? Sign Up, it's FREE!
Guest

ArduinoCode.pde

By: a guest on Apr 3rd, 2013  |  syntax: None  |  size: 5.15 KB  |  views: 8  |  expires: Never
download  |  raw  |  embed  |  report abuse  |  print
Text below is selected. Please press Ctrl+C to copy to your clipboard. (⌘+C on Mac)
  1. #include <Servo.h>
  2. #include "serial.h"
  3. // Arduino Code for 2012 competition
  4.  
  5. #define CLAW 3
  6. #define ARM 5
  7. #define LEFTDOOR 7
  8. #define RIGHTDOOR 6
  9. #define LEFTMOTOR 10
  10. #define RIGHTMOTOR 11
  11.  
  12. #define BUTTONPIN A1
  13. #define IRPIN A3
  14. #define LEDPIN 13
  15.  
  16. #define CLAWOPEN 80
  17. #define CLAWCLOSED 167
  18. #define CLAWSTOP 127
  19.  
  20. #define ARMDOWN 99      
  21. #define ARMDOWNINIT 50
  22. #define ARMUP 155
  23. #define ARMSTOP 127
  24.  
  25. #define LEFTOPEN 0
  26. #define LEFTCLOSED 90
  27. #define RIGHTOPEN 255
  28. #define RIGHTCLOSED 120
  29.  
  30. #define IRTHRESH 20
  31. #define BUTTONTHRESH 10
  32.  
  33. int foo = 1;
  34.  
  35. void arduinoInit();
  36. void set_servo_position(int s, unsigned char position);
  37. int get_sw1();  
  38. void led_on();
  39. void led_off();
  40. int analog(int num);
  41.  
  42.  
  43. void openClaw();
  44. void closeClaw();
  45. void liftCans();
  46. void grabFirst();
  47. void lowerClaw();
  48. void raiseClaw();
  49. void openWings();
  50. void closeWings();
  51. void openClawAll();
  52. void setSpeed(char motor, int speed);
  53.  
  54.  
  55. int can = 0;
  56. int button = 0;
  57.  
  58.  
  59.  
  60.  
  61.  
  62. //Servos
  63. Servo servos[15];
  64.  
  65. int main()
  66.  
  67. {
  68.   arduinoInit();
  69.   int i;
  70.   int x = 0;
  71.   char y = 0;
  72.   can = 0;
  73.   button = 0;
  74.   openWings();
  75.   set_servo_position(ARM, ARMSTOP);
  76.   set_servo_position(CLAW, CLAWSTOP);
  77.   delay(1000);
  78.   lowerClaw();
  79.   delay(50);
  80.   raiseClaw();
  81.   openClawAll();        
  82.  
  83.   while(true)   //read serial if its there and check button and switch
  84.   {    
  85.     if(get_sw1() == 1 || button == 1)
  86.     {
  87.       button = 1;      
  88.       led_on();        
  89.     }
  90.     else
  91.     {
  92.       led_off();
  93.     }
  94.     if(analog(IRPIN) <= IRTHRESH)       //TODO read analog
  95.     {
  96.       can = 1;
  97.     }
  98.     if(Serial.available() > 0)
  99.     {
  100.        execute_function();
  101.     }
  102.   }
  103. }
  104.  
  105.  
  106.  
  107. void arduinoInit()
  108.  
  109. {      
  110.   init();
  111.   servos[LEFTMOTOR].attach(LEFTMOTOR, 1000, 2000);
  112.   servos[RIGHTMOTOR].attach(RIGHTMOTOR, 1000, 2000);
  113.   servos[CLAW].attach(CLAW, 1000, 2000);
  114.   servos[ARM].attach(ARM, 1000, 2000);
  115.   servos[LEFTDOOR].attach(LEFTDOOR, 1000, 2000);
  116.   servos[RIGHTDOOR].attach(RIGHTDOOR, 1000, 2000);
  117.  
  118.   pinMode(LEDPIN, OUTPUT);      //set LED pin to output
  119.  
  120.   Serial.begin(BAUD);
  121. }
  122.  
  123.  
  124.  
  125. void set_servo_position(int s, unsigned char position)
  126.  
  127. {
  128.   int towrite = position*(12.0/17.0);
  129.   printf("Writing %d to motor %d\n", towrite, position);
  130.   Serial.print("Writing ");
  131.   Serial.print((int)position);
  132.   Serial.print("to motor ");
  133.   Serial.println(s);
  134.   servos[s].write(towrite);
  135. }
  136.  
  137.  
  138.  
  139. int get_sw1()
  140.  
  141. {
  142.   int sum =  analogRead(BUTTONPIN);
  143.  
  144.   if(sum > 0)
  145.     return 0;
  146.    
  147.   button = 1;
  148.   return 1;
  149. }
  150.  
  151. void led_on()
  152. {
  153.   digitalWrite(LEDPIN, HIGH);
  154. }
  155.  
  156.  
  157. void led_off()
  158. {
  159.   digitalWrite(LEDPIN, LOW);
  160. }
  161.  
  162.  
  163. int analog(int num)
  164. {
  165.   int x = 0;
  166.   int sum = 0;
  167.  
  168.   for(x = 0; x < 100; x++)
  169.   {
  170.     sum += analogRead(num);
  171.   }
  172.   return sum;
  173. }
  174.  
  175.  
  176. void setSpeed(char motor, int speed)
  177. { //175 to 255 and 100 to 0
  178.  
  179.   int x;
  180.  
  181.   if(speed >= 0)
  182.     set_servo_position(motor, x = 100-speed);
  183.    
  184.   if(speed <  0)
  185.     set_servo_position(motor, x = (175 - speed*.8));
  186. }
  187.  
  188.  
  189.  
  190.  
  191.  
  192. //Closes claw //DONE
  193.  
  194. void closeClaw()
  195. {
  196.   set_servo_position(CLAW, CLAWCLOSED);
  197.   delay(400); //TODO FIND THIS VALUE
  198.   set_servo_position(CLAW, CLAWSTOP);
  199. }
  200.  
  201. //DONE
  202.  
  203. void openClaw()
  204. {
  205.   set_servo_position(CLAW, CLAWOPEN);
  206.   delay(700); //TODO FIND THIS VALUE
  207.   set_servo_position(CLAW, CLAWSTOP);
  208. }
  209.  
  210. void openClawAll()
  211. {
  212.   set_servo_position(CLAW, CLAWOPEN);
  213.   delay(700); //TODO FIND THIS VALUE
  214.   set_servo_position(CLAW, CLAWSTOP);
  215. }
  216.  
  217. void lowerClaw()
  218. {
  219.   int x = 0;
  220.   set_servo_position(ARM, ARMDOWNINIT);
  221.   delay(300);
  222.   set_servo_position(ARM, ARMDOWN);
  223.   delay(700);
  224.   set_servo_position(ARM, ARMSTOP);
  225. }
  226.  
  227.  
  228.  
  229. void raiseClaw()
  230. {
  231.   set_servo_position(ARM, ARMUP);
  232.   delay(800); //TODO find value to raise
  233.   set_servo_position(ARM, ARMSTOP);
  234. }
  235.  
  236.  
  237.  
  238. //lower arm, grab cans, lift arm
  239. //For lifting every can but first
  240. void liftCans()  
  241. {
  242.   set_servo_position(ARM, 84);
  243.   delay(300);
  244.   set_servo_position(ARM, ARMSTOP);
  245.   openClaw();
  246.   lowerClaw();
  247.   closeClaw();
  248.   raiseClaw();
  249.   can = 0;
  250.   Serial.write(LIFT_CANS);
  251. }
  252.  
  253.  
  254.  
  255. //GRAB_FIRST
  256.  
  257. void grabFirst()
  258. {
  259.   closeWings();
  260.   set_servo_position(ARM, 84);
  261.   delay(300);
  262.   set_servo_position(ARM, ARMSTOP);
  263.   openClaw();
  264.   lowerClaw();
  265.   closeClaw();
  266.   raiseClaw();
  267.   can = 0;
  268.   Serial.write(GRAB_FIRST);
  269. }
  270.  
  271. //SCORE CANS
  272.  
  273. void scoreCans()
  274. {
  275.   set_servo_position(ARM, 84);
  276.   delay(300);
  277.   set_servo_position(ARM, ARMSTOP);
  278.  
  279.   lowerClaw();
  280.  
  281.   openClawAll();
  282.  
  283.   openWings();
  284.  
  285.   raiseClaw();
  286.   Serial.write(SCORE);
  287. }
  288.  
  289.  
  290.  
  291. char isCan()    
  292. {
  293.   //if(digital(0) == 0 || can == 1)
  294.   if(analog(IRPIN) <= IRTHRESH|| can == 1)
  295.   {
  296.     can = 0;
  297.     return CAN;
  298.   }    
  299.   else
  300.       return NOCAN;
  301. }
  302.  
  303. int isButton()  
  304. {
  305.   if(get_sw1() || button == 1)
  306.   {
  307.     button = 0;
  308.    return BUTTON;
  309.     delay(300);
  310.   }    
  311.   else
  312.       return NOBUTTON;
  313. }
  314.  
  315.  
  316. void openWings()
  317. {
  318.   set_servo_position(LEFTDOOR, LEFTOPEN);
  319.   set_servo_position(RIGHTDOOR, RIGHTOPEN);
  320. }
  321.  
  322.  
  323.  
  324. void closeWings()
  325. {
  326.   set_servo_position(LEFTDOOR, LEFTCLOSED);
  327.   set_servo_position(RIGHTDOOR, RIGHTCLOSED);  
  328. }
  329.  
  330.  
  331. void reverse()
  332. {
  333.   setSpeed(0, -100);
  334.   setSpeed(1, -100);
  335. }
clone this paste RAW Paste Data