Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <Servo.h>
- #include "serial.h"
- // Arduino Code for 2012 competition
- #define CLAW 3
- #define ARM 5
- #define LEFTDOOR 7
- #define RIGHTDOOR 6
- #define LEFTMOTOR 10
- #define RIGHTMOTOR 11
- #define BUTTONPIN A1
- #define IRPIN A3
- #define LEDPIN 13
- #define CLAWOPEN 80
- #define CLAWCLOSED 167
- #define CLAWSTOP 127
- #define ARMDOWN 99
- #define ARMDOWNINIT 50
- #define ARMUP 155
- #define ARMSTOP 127
- #define LEFTOPEN 0
- #define LEFTCLOSED 90
- #define RIGHTOPEN 255
- #define RIGHTCLOSED 120
- #define IRTHRESH 20
- #define BUTTONTHRESH 10
- int foo = 1;
- void arduinoInit();
- void set_servo_position(int s, unsigned char position);
- int get_sw1();
- void led_on();
- void led_off();
- int analog(int num);
- void openClaw();
- void closeClaw();
- void liftCans();
- void grabFirst();
- void lowerClaw();
- void raiseClaw();
- void openWings();
- void closeWings();
- void openClawAll();
- void setSpeed(char motor, int speed);
- int can = 0;
- int button = 0;
- //Servos
- Servo servos[15];
- int main()
- {
- arduinoInit();
- int i;
- int x = 0;
- char y = 0;
- can = 0;
- button = 0;
- openWings();
- set_servo_position(ARM, ARMSTOP);
- set_servo_position(CLAW, CLAWSTOP);
- delay(1000);
- lowerClaw();
- delay(50);
- raiseClaw();
- openClawAll();
- while(true) //read serial if its there and check button and switch
- {
- if(get_sw1() == 1 || button == 1)
- {
- button = 1;
- led_on();
- }
- else
- {
- led_off();
- }
- if(analog(IRPIN) <= IRTHRESH) //TODO read analog
- {
- can = 1;
- }
- if(Serial.available() > 0)
- {
- execute_function();
- }
- }
- }
- void arduinoInit()
- {
- init();
- servos[LEFTMOTOR].attach(LEFTMOTOR, 1000, 2000);
- servos[RIGHTMOTOR].attach(RIGHTMOTOR, 1000, 2000);
- servos[CLAW].attach(CLAW, 1000, 2000);
- servos[ARM].attach(ARM, 1000, 2000);
- servos[LEFTDOOR].attach(LEFTDOOR, 1000, 2000);
- servos[RIGHTDOOR].attach(RIGHTDOOR, 1000, 2000);
- pinMode(LEDPIN, OUTPUT); //set LED pin to output
- Serial.begin(BAUD);
- }
- void set_servo_position(int s, unsigned char position)
- {
- int towrite = position*(12.0/17.0);
- printf("Writing %d to motor %d\n", towrite, position);
- Serial.print("Writing ");
- Serial.print((int)position);
- Serial.print("to motor ");
- Serial.println(s);
- servos[s].write(towrite);
- }
- int get_sw1()
- {
- int sum = analogRead(BUTTONPIN);
- if(sum > 0)
- return 0;
- button = 1;
- return 1;
- }
- void led_on()
- {
- digitalWrite(LEDPIN, HIGH);
- }
- void led_off()
- {
- digitalWrite(LEDPIN, LOW);
- }
- int analog(int num)
- {
- int x = 0;
- int sum = 0;
- for(x = 0; x < 100; x++)
- {
- sum += analogRead(num);
- }
- return sum;
- }
- void setSpeed(char motor, int speed)
- { //175 to 255 and 100 to 0
- int x;
- if(speed >= 0)
- set_servo_position(motor, x = 100-speed);
- if(speed < 0)
- set_servo_position(motor, x = (175 - speed*.8));
- }
- //Closes claw //DONE
- void closeClaw()
- {
- set_servo_position(CLAW, CLAWCLOSED);
- delay(400); //TODO FIND THIS VALUE
- set_servo_position(CLAW, CLAWSTOP);
- }
- //DONE
- void openClaw()
- {
- set_servo_position(CLAW, CLAWOPEN);
- delay(700); //TODO FIND THIS VALUE
- set_servo_position(CLAW, CLAWSTOP);
- }
- void openClawAll()
- {
- set_servo_position(CLAW, CLAWOPEN);
- delay(700); //TODO FIND THIS VALUE
- set_servo_position(CLAW, CLAWSTOP);
- }
- void lowerClaw()
- {
- int x = 0;
- set_servo_position(ARM, ARMDOWNINIT);
- delay(300);
- set_servo_position(ARM, ARMDOWN);
- delay(700);
- set_servo_position(ARM, ARMSTOP);
- }
- void raiseClaw()
- {
- set_servo_position(ARM, ARMUP);
- delay(800); //TODO find value to raise
- set_servo_position(ARM, ARMSTOP);
- }
- //lower arm, grab cans, lift arm
- //For lifting every can but first
- void liftCans()
- {
- set_servo_position(ARM, 84);
- delay(300);
- set_servo_position(ARM, ARMSTOP);
- openClaw();
- lowerClaw();
- closeClaw();
- raiseClaw();
- can = 0;
- Serial.write(LIFT_CANS);
- }
- //GRAB_FIRST
- void grabFirst()
- {
- closeWings();
- set_servo_position(ARM, 84);
- delay(300);
- set_servo_position(ARM, ARMSTOP);
- openClaw();
- lowerClaw();
- closeClaw();
- raiseClaw();
- can = 0;
- Serial.write(GRAB_FIRST);
- }
- //SCORE CANS
- void scoreCans()
- {
- set_servo_position(ARM, 84);
- delay(300);
- set_servo_position(ARM, ARMSTOP);
- lowerClaw();
- openClawAll();
- openWings();
- raiseClaw();
- Serial.write(SCORE);
- }
- char isCan()
- {
- //if(digital(0) == 0 || can == 1)
- if(analog(IRPIN) <= IRTHRESH|| can == 1)
- {
- can = 0;
- return CAN;
- }
- else
- return NOCAN;
- }
- int isButton()
- {
- if(get_sw1() || button == 1)
- {
- button = 0;
- return BUTTON;
- delay(300);
- }
- else
- return NOBUTTON;
- }
- void openWings()
- {
- set_servo_position(LEFTDOOR, LEFTOPEN);
- set_servo_position(RIGHTDOOR, RIGHTOPEN);
- }
- void closeWings()
- {
- set_servo_position(LEFTDOOR, LEFTCLOSED);
- set_servo_position(RIGHTDOOR, RIGHTCLOSED);
- }
- void reverse()
- {
- setSpeed(0, -100);
- setSpeed(1, -100);
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement