#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);
}