/* For backwards compatability */
void servo_control(unsigned char value)
{
if (value == ENABLE)
servo_init();
else
servo_setup(DISABLE);
}
/* Sets up servo control on default ports D2 and D3 */
void servo_init(void)
{
servo_setup(ENABLE);
servo_port(0, ENABLE);
servo_port(1, ENABLE);
sei();
}
int main(){
/*===Start===*/
servo_setup();
printf("Press any button to start");
WAIT(a_button()||b_button()||c_button());
//printf("waiting 2 seconds"); sleep(2);
/*===Gold One===*/
servo_drive_pos();
squareup(-80,1);
forward(35,100);
squareup(80,1);
get_gold();
/*===Score Gold===*/
back(17,80);
score_gold();
/*===Reposition===*/
left(90,0,100);
squareup(80,3);
/*===Botguy===*/
thread going=thread_create(arms_down_slow);
thread_start(going);
back(20,100);
back(30,100);
/*set_servo_position(ARM,ARM_UP);
msleep(500);
forward(34+5,100);
back(5);
set_servo_position(ARM,ARM_DOWN);
msleep(500);*/
/*===Tail===*/
disable_servos();
ao();
}
void setup() {
Serial.begin(19200);
servo_setup();
}
int main()
{
// initialize the LED pin as an output
LPC_GPIO1->FIODIR = 1 << 18 | 1 << 27;
LPC_GPIO1->FIOSET = 1 << 18 | 1 << 27;
LPC_GPIO0->FIODIR = 1 << 4 | 1 << 5 | 1 << 6;
servo_setup(); //Initialize servo
servo_steer(0.0);
OFF;
//21ms on
//6.5ms off
//2.5ms on
//6.5ms off
//2.5ms on
//9 ms off
//2.5 on
//6.5 off
//2.5 on
//2.5 off
//7 on
//16ms off
int i = 0;
int c = 0;
while(1){
printf("hello");
c = getchar();
if(c == 'o'){
ON;
delay_ms(20);
OFF;
delay_ms(7);
ON;
delay_ms(2.6);
OFF;
delay_ms(7);
ON;
delay_ms(2.6);
OFF;
delay_ms(9);
ON;
delay_ms(2.6);
OFF;
delay_ms(6);
ON;
delay_ms(2.5);
OFF;
delay_ms(2.2);
ON;
delay_ms(7);
OFF;
delay_ms(16);
if(i == 100){
delay_ms(500);
}
if(i >= 200){
delay_ms(100);
i = 0;
c = 0;
} else {
i++;
}
}
}
return 0;
}
int main()
{
LPC_GPIO1->FIODIR = 1 << 18 | 1 << 27;
LPC_GPIO0->FIODIR = 1 << 4 | 1 << 5 | 1 << 6;
rgb_setup();
rgb_set(0x00,0xFF,00);
motor_setup();
receiver_setup();
servo_setup(); //Initialize servo
servo_steer(0);
rgb_set(0xFF,0,0);
int throttle = 0;
int steering = 0;
int rudder = 0;
int elevator = 0;
int speed = 0;
int servo_position = .5;
int stop = 0;
while(1){
throttle = radio_data[0];
steering = radio_data[1];
rudder = radio_data[2];
elevator = radio_data[3];
if(throttle > 48000){
throttle = 48000;
}
if(throttle < 24000){
stop = 1;
throttle = 24000;
} else {
stop = 0;
}
steering = clip(steering);
rudder = clip(rudder);
elevator = clip(elevator);
speed = (throttle-24000) / 2400;
servo_position = (steering-24000) / 2400;
printf("1:[");
int i = 0;
while(i < 11){
if(i == speed){
printf("|");
} else {
printf(" ");
}
i++;
}
printf("] 2:[");
i = 0;
while(i < 11){
if(i == servo_position){
printf("o");
} else {
printf(" ");
}
i++;
}
printf("] 3:[");
i = 0;
while(i < 11){
if(i == (rudder-24000) / 2400){
printf("|");
} else {
printf(" ");
}
i++;
}
printf("] 4:[");
i = 0;
while(i < 11){
if(i == (elevator-24000) / 2400){
printf("|");
} else {
printf(" ");
}
i++;
}
if(radio_data[4] < 32000){
printf("][v]");
} else if(radio_data[4] < 40000){
printf("][-]");
} else {
printf("][^]");
}
if(radio_data[5] < 32000){
printf("[v]");
} else if(radio_data[5] < 40000){
printf("[-]");
} else {
printf("[^]");
}
if(radio_data[6] < 32000){
printf("[v]");
} else if(radio_data[6] < 40000){
printf("[-]");
} else {
printf("[^]");
}
if(radio_data[7] < 32000){
printf("[v]");
} else if(radio_data[7] < 40000){
printf("[-]");
} else {
printf("[^]");
}
printf("\r\n");
delay_ms(5);
}
return 0;
}
