void Start() //Misc. set up functions
{
create_connect();
set_create_total_angle(0);
//wait_for_light(LightPort);
shut_down_in(RunTime); //TEST THIS
}
int main()
{
camera_open();
ssp(SORT_SERVO,START);
ssp(RED_SERVO,RED_START);
ssp(GREEN_SERVO,GREEN_START);
wait_for_light(0);
start();
shut_down_in(119);
enable_servos();
msleep(5000);
reset();
right(47,0);
forward(35);
right(10,0);
backward(46);
full_sort();
other_side();
ssp(GREEN_SERVO,GREEN_DUMP);
msleep(1000);
ssp(GREEN_SERVO,GREEN_START);
msleep(250);
now();
ao();
disable_servos();
}
void Start (void)
{
wait_for_light(LightPort);
shut_down_in (117); //TEST THIS
create_connect();
set_create_total_angle(0);
}
void StartUp() {
enable_servos();
set_servo_position(ARM_PORT, ARM_DOWN);
set_servo_position(CARDBOARD_PORT, CARDBOARD_START);
set_servo_position(2, 800);
msleep(300);
wait_for_light(5);
shut_down_in(119);
}
int main()
{
printf("Mini Bot!\n");
enable_servos();
set_servo_position(ARM_SERVO, ARM_UP);
printf("Waiting for light!\n");
while (analog(0) > 700 && a_button() == 0)
{
msleep(10);
}
shut_down_in(118.0);
msleep (25000);
go(90, 90);
msleep(6700);
stop();
msleep(100);
printf("stopped\n");
set_servo_position(ARM_SERVO, ARM_DOWN);
printf("arm down\n");
go(-80,-80);
msleep(500);
stop();
go(-90, 90);
msleep(1150);
stop();
go(90, 90);
msleep(2500);
stop();
set_servo_position(ARM_SERVO, ARM_UP);
go(90, 90);
msleep(2500);
stop();
msleep(1000000);
return 0;
}
int main() {
wait_for_light(LIGHT_SENS);
//shake();
//startingT();
start();
shut_down_in(119);
Abyssal_Voyage();
Chomp();
now();
Acquired_Taste();
Devour();
Tougue_Lash();
Acquired_Taste();
//dance();
now();
}
void start(int servo_position)
{
int k;
camera_open();
for (k = 0; k < 10; k = k+1)
{
camera_update();
msleep(200);
}
set_servo_position(1, servo_position);
enable_servos();
msleep(1000);
wait_for_light(7);
shut_down_in(115);
}
int main() {
printf("In Drivepath test\n");
//light_start(0);
shut_down_in(119.5);
arm_drive();
claw_open();
bin_down();
enable_servos();
//forward(100);
forward(10);
int green = 0; //whether green has been collected
int red = 0; //whether red has been collected
start(); //start timer
int pile1Result = pom_collection();
backward(pom_collection_turn * 5);
pom_collection_turn = 0; //reset turn
if(pile1Result == 0) {
green = 1;
printf("Collecting first pile of green poms\n");
}
else if(pile1Result == 1) {
red = 1;
printf("Collecting first pile of red poms\n");
}
//backward(5);
left(15, ks/2);
off(MOT_RIGHT);
off(MOT_LEFT);
collect_poms();
collect_poms();
msleep(1000);
disable_servos();
ao();
return 0;
}
// Created on Sat April 13 2013
// This is a transport move test.
int main()
{
wait_for_light(3); // start the robot after the light is turned on
shut_down_in(120); // shutdown the robot in 2 Minutes
enable_servo();
printf("Hello, World!\n");
set_servo_position(1, 0);
motor(1,100); // move the right wheel
motor(3,100); // move the left wheel
msleep(3500); // move for 3.5 seconds
ao();
set_servo_position(1, 2048); // kick bot guy out
motor(1,-100);
motor(3,-100);
disable_servo();
return 0;
}
//start 48500
//down 58000
void main()
{
set_servo_position(Servo_Back,Servo_Back_Up);
set_servo_position(Servo_Left,Servo_Left_Closed);
set_servo_position(Servo_Right,Servo_Right_Closed);
enable_servos();
calibrate();
//wait for light
//printf("wait for light oida");
//set_b_button_text("I am the Twilight");
//while(!b_button()){}
wait_for_light(Sensor_Light);
//shutdown stuff
shut_down_in(115);
start=seconds();
//start position for create
start_position();
//we see everything
camera_open();
//drive in front of the cubes
take_position();
//watch for cubes
cube_is_near();
ao();
//bring the first cube to the pipes
bringback();
//take the 2nd cube and bring them to the pipes
bringback2cube();
}
int main()
{
/* FIRST CALIBRATION: based on poms position adjust starting position
*/
printf("test 3.11, calibration value: \nclaw down : %d, \nright ang forward right: %d, \nright ang forward left : %d, \nback right angle %d ...\n", DOWN_SERVO, RIGHT_ANGLE_CLICKS_MRP_RIGHT, RIGHT_ANGLE_CLICKS_MRP_LEFT, RIGHT_ANGLE_CLICKS_BACK_MRP);
double start_time = seconds();
enable_servos();
clear_motor_position_counter(RIGHT_MOTOR);
clear_motor_position_counter(LEFT_MOTOR);
wait_for_light(1);
shut_down_in(118);
clawUp();
//**************************************
//* PART 1 : get first set of poms to lower storage area
//*
//**************************************
moveBackward(1, NO_DEBUG);
moveForwardHighSpeed(23, DEBUG);
clawDown();
//moveForward(8,NO_DEBUG);
//we have the Poms
rightAngleBwd(RIGHT, NO_DEBUG);
//bump against the upper storage area
moveBackwardHighSpeed(15, NO_DEBUG);
moveForwardHighSpeed(17, NO_DEBUG);
rightAngleBwd(RIGHT, NO_DEBUG);
//bump against upper PVC side
moveBackwardHighSpeed(18, NO_DEBUG);
//move towards dropping poms
moveForward(1.5, NO_DEBUG);
msleep(300);
rightAngleFwd(LEFT, NO_DEBUG);
msleep(300);
clawUp();
//uses IR sensor to stop the forward movement
moveForwardTilBlackLine(15, NO_DEBUG);
printf("====> elapsed time end of part 1: %f\n", (seconds() - start_time));
//**************************************
//* END OF PART 1 : get first set of poms to lower storage area
//*
//**************************************
//**************************************
//* PART 2 : get the blue cube to the corner
//*
//**************************************
moveBackwardHighSpeed(25, NO_DEBUG);
//recalibrate against top PVC next to botguy
rightAngleBwd(LEFT, NO_DEBUG);
moveBackwardHighSpeed(11, NO_DEBUG);
// SECOND CALIBRATION: based on blackline/blue position
moveForward(7.5, NO_DEBUG);
rightAngleFwd(RIGHT, NO_DEBUG);
//get the cube now
//====>>>>> may have to be adjust the day of competition based on position of 2nd
//====>>>>> set of poms
moveForwardHighSpeed(15, NO_DEBUG);
//====>>>>>
clawAlmostDownCube();
twentyTwoAngleFwd(LEFT, NO_DEBUG);
clawUp();
//====>>>>> calibration to get to the corner 22.5 fwd, 22.5 more forward, aiming at bottom left hand corner
moveForwardHighSpeed(2.5, NO_DEBUG);
twentyTwoAngleFwd(LEFT, NO_DEBUG);
//clawAlmostDownCube();
moveForwardHighSpeed(30, NO_DEBUG);
//====>>>>>
//moveForwardHighSpeed(2, NO_DEBUG);
//====>>>>>
moveBackwardHighSpeed(9, NO_DEBUG);
fortyFiveAngleFwd(LEFT, NO_DEBUG);
printf("====> elapsed time end of part 2: %f\n", (seconds() - start_time));
//**************************************
//* END OF PART 2 : get the blue cube in the corner
//*
//**************************************
//**************************************
//* PART 3 : prepare to get second set of poms
//*
//**************************************
//recalibrate against side PVC
rightAngleBwd(RIGHT, NO_DEBUG);
moveBackwardHighSpeed(25, NO_DEBUG);
moveForward(5, NO_DEBUG);
//calibrating with top PVC
rightAngleBwd(LEFT,NO_DEBUG);
moveBackwardHighSpeed(23,NO_DEBUG);
// THIRD CALIBRATION: based on second set of Poms
//below is the number of inches from the side PVC
//
moveForwardHighSpeed(10, NO_DEBUG);
rightAngleBwd(RIGHT, NO_DEBUG);
//recalibrate with side pvc
moveBackwardHighSpeed(5,NO_DEBUG);
printf("====> elapsed time end of part 3: %f\n", (seconds() - start_time));
//**************************************
//* END OF PART 3 : prepare to get second set of poms
//*
//**************************************
//**************************************
//* PART 4 : bring second set of poms home
//*
//**************************************
//grabs the poms, bring down claw and go home
moveForwardHighSpeed(12.5,NO_DEBUG);
clawDown();
moveForwardHighSpeed(59.5, NO_DEBUG);
//here is the code to recalibrate one more time... but do we have enough time?
/*//recalibrate against top PVC
rightAngleBwd(LEFT,NO_DEBUG);
moveBackwardHighSpeed(10, NO_DEBUG);
//move towards dropping poms
moveForwardHighSpeed(10, NO_DEBUG);
//msleep(500);
printf("==> moving righ angle bwd\n");
rightAngleFwd(LEFT, NO_DEBUG);
*/
//recalibrate against top PVC
//moveForwardHighSpeed(41,NO_DEBUG);
//last calibration before dropping second set of poms
rightAngleBwd(LEFT,NO_DEBUG);
moveBackwardHighSpeed(14, NO_DEBUG);
//move towards dropping poms
moveForwardSlow(1.5, NO_DEBUG);
msleep(300);
rightAngleFwd(LEFT, NO_DEBUG);
msleep(300);
clawUp();
moveForwardTilBlackLine(15, DEBUG);
//**************************************
//* END OF PART 4 : bring second set of poms home
//*
//**************************************
printf("====> elapsed time: %f\n", (seconds() - start_time));
ao();
disable_servos();
return 0;
}
void main (int argc, char ** argv)
{
int loop_done = 0;
int task_A_done = 0;
int task_B_done = 0;
int angle = 0;
int dangle = 0;
int angle0 = 0;
int distance0 = 0;
int distance = 0 ;
int ddistance = 0;
// float lf_args[20];
// read inputs
// printf("Args: %d\n", argc);
// for (i = 2; i < argc; i++)
// {
// scanf("%lf%, argv[i]
// }
//get_create_total_ angle (.1);
//create_drive_direct (left speed, right speed)\
//move_to_position (motor #, motor power(-for backwards), end position)
//get_motor_done (motor#)
//clear_motor_position_counter (motor#)
//msleep (seconds)
//i'll probably use this often
//to start by light, do:
//while(analog(1) > 200)
if (0)
{
//we have seen the light
msleep(100);
if(analog(1) < 800){
shut_down_in(119);
}
}
create_connect ();
clear_motor_position_counter (0);
enable_servo (0);
set_servo_position (0 ,1300);
//set_servo_position (1800);
// ------------------------------------------------------------------------
//step:1 aim the arm to knock over the box
//raise arm
printf("-----Step 1-----\n");
move_motor (0, -900, -1200);
// swing arm to hit the box, then get back to position
spin (50, -50, 20);
spin (-50, 50, 20);
create_stop ();
// ------------------------------------------------------------------------
//step:2 knock over box and turn
//lift arm
//turn 90
printf("-----Step 2-----\n");
move_to_position (0, -900, -2600);
msleep (900);
//turn create in a circular position and raise arm so it doesn't hit anything
angle0 = get_create_total_angle (.05);
angle = angle0;
dangle = abs (angle0 - angle);
printf("a0: %d, a: %d, da: %d, d: %d\n", angle0, get_create_total_angle (.05), dangle , task_A_done);
create_drive_direct (-178,-648);
while(! loop_done)
{
if (dangle >= 155)
{
task_A_done = 1;
create_stop ();
}
else if (dangle >= 125)
{
create_drive_direct (-120, -181);
}
if (get_motor_done (0))
{
task_B_done = 1;
}
if (task_A_done && task_B_done)
{
loop_done = 1;
}
msleep(100);
angle = get_create_total_angle (.05);
dangle = abs (angle0 - angle);
printf("a0: %d, a: %d, da: %d, d: %d\n", angle0, get_create_total_angle (.05), dangle , task_A_done);
}
create_stop ();
// ------------------------------------------------------------------------
//step:4 aim claw to point of botguy
//turn 20
//lower arm
printf("-----Step 4-----\n");
//--tweaking position so it gets in just the right position
move_to_position (0, 400, -900);
move (20, 20, 50);
set_servo_position (0 ,400);
create_drive_straight (-30);
move_motor (0, 600, -300);
create_drive_straight (-38);// at this point, botguy is in his palm
//can't remember function to program
msleep (3000);
loop_done = 0;
if (! loop_done)
{
create_stop ();
if (! digital (10))
{
move_motor (0, 400, -900);
spin (20, -20, 5);
move_motor (0, 600, -300);
create_stop ();
loop_done = 1;
}
else
{
loop_done = 1;
}
}
// ------------------------------------------------------------------------
//step:5 grab botguy and lift him up (not complete)
printf("-----Step 5-----\n");
move (-10, -10, 10);
msleep (1300);
//spin (60, -60, 5);
//close arm and grab botguy
set_servo_position (0, 1510);
msleep (400);
move_to_position (0, -1000, -3000);
//wait for him to finish
msleep (5000);
// ------------------------------------------------------------------------
//step:6 move backwards till it's in position
printf("-----Step 6-----\n");
//back up in an arc
move (130, 280, 780);
//move towards the center and lower drop height
move_to_position (0, -900, -500);
create_drive_straight (-20);
msleep (3000);
//drop botguy in transport
set_servo_position (0, 200);
create_stop ();
//start moving towards the pipe
move_motor (0, 500, -1200);
spin (50, -50, 70);
move (50, 50, 100);
spin (-50, 50, 70);
/*
if (! loop_done)
{
if (get_object_count(2) == 1)
{
create_stop ();
spin(-10, 10, 5);
create_stop ();
move(20, 20, 20);
create_stop ();
set_servo_position (0, 1400);
loop_done = 1;
}
}
loop_done = 0;
spin (10, 10, 5);
set_servo_position (0, 200);
*/
/*
spin (50, -50, 85);
move_motor (0, 400, -400);
move (-100, -100, 50);
loop_done = 0;
//close arm on pipe
set_servo_position (0, 1510);
//turn arm to face the pole
spin (-50, 50, 50);
//up, forward, then drop pipe
move_motor (0, 400, -2300);
move (-100, -100, 100);
set_servo_position (0, 200);
ao ();
*/
create_disconnect ();
}
int main()
{
double start = 0.0;
// initialize servo, camera, etc here?.
// if you are using a Create you should uncomment the next line
//create_connect();
enable_servos ();
set_servo_position(3, ARM_UP);
//goto GO_DOWN_CAVE;
wait_for_light(2); // change the port number to match where your robot
shut_down_in(119); // shut off the motors and stop the Create after 119 seconds
// note that shut_down_in should immediately follow wait_for_light!!
printf("My program should score more points than this template!\n");
// replace printf above with your code
//create_disconnect();
// Created on Mon March 2 2015
int a1;
int a2;
//turn on robot
motor(0,-700);
motor(2,-700);
msleep(700);
#if 0
//go forward
motor(0,400);
motor(2,400);
msleep(1000);
//turn to aline
motor(0, -100);
motor(2, 100);
msleep(500);
//aline robot
motor(0,-100);
motor(2,-100);
msleep(2000);
go(0, 0);
msleep(1500);
#else
//go forward
motor(0,400);
motor(2,400);
msleep(400);
//turn to aline
motor(0,0);
motor(2,200);
msleep(1300);
//aline robot
motor(0,-100);
motor(2,-100);
msleep(2000);
#endif
// ao(); return 0;
//go forwad
motor(0,85);
motor(2,85); //tweek if wheel goes sideways
//msleep(6000);
printf("before 3000 delay\n");
msleep (1500);
set_servo_position(3, ARM_UP_PEG);
printf("after 3000 delay\n");
msleep(400);
printf("%d\n",analog(1));
start=seconds ();
while ((a1 = analog(1)) <500) {
// printf("%d\n",a1);
//msleep(5);
if(seconds()-start>3.5){
printf("time_out\n");
break;
}
}
printf("Found black %d\n", a1);
//turn to go into middle
turn_right();
//go forward
motor(0,60);
motor(2,60);
printf("go forward\n");
//msleep(2000);
while ((a1 = analog(1)) <600 || (a2 = analog(3)) <600) {
// printf("%4d %4d\n",a1, a2);
//msleep(5);
if (seconds()-start >10.5) {
break;
}
}
printf("%4d %4d\n",a1, a2);
//turn in cave
motor(0,0);
motor(2,0);
turn_right();
set_servo_position(3, ARM_UP);
//go to pick up cubes
//motor(0,75);
//motor(2,0);
printf("turn right\n");
//msleep(1500);
//line follow
start=seconds ();
while(1){
if(seconds()-start>4.9){
break;
}
if (analog(1)>500) {
motor(0,80);
motor(2,100);
}else if (analog(3)>500) {
motor(0,100);
motor(2,80);
}else {
motor(0,100);
motor(2,100);
}
}
//motor(0,85);
//motor(2,85);
//msleep(5500);
//put arm downto grab
GO_DOWN_CAVE:
go(0,0);
set_servo_position(3, ARM_DOWN);
msleep(500);
//begining of picking up poms
go(75,75);
msleep(2500);
go_for(-50,-90, 1000); // back up/turn/pull pom-poms
go_for(90, -90, 500);
go_for(-100, -90, 600);
set_servo_position(3, ARM_UP);
go_for(100, 100, 2000);
go_for(-100, -100,2000);
set_servo_position(3, ARM_DOWN);
msleep(700);
go_for(100,100,1000);
go(-10, -10);
set_servo_position(3, ARM_DOWN);
msleep(100);
set_servo_position(3, ARM_DOWN + 50);
msleep(100);
set_servo_position(3, ARM_DOWN + 100);
msleep(100);
set_servo_position(3, ARM_DOWN + 150);
msleep(100);
set_servo_position(3, ARM_DOWN + 200);
msleep(100);
//set_servo_position(3, ARM_DOWN + 250);
//msleep(100);
//set_servo_position(3, ARM_DOWN + 400);
//msleep(100);
//push the poms over the top of the PVC
go_for(20, 20, 1000);
//StOp
go(0,0);
//wiggle the arm up and down so the poms fall off
set_servo_position(3, ARM_DOWN+350);
msleep(250);
set_servo_position(3, ARM_DOWN+300);
msleep(250);
set_servo_position(3, ARM_DOWN+350);
msleep(250);
// Raise the arm
set_servo_position(3, ARM_UP);
msleep(300);
go_for(0, -100, 1000);
//line follow
start=seconds ();
while(1){
if(seconds()-start>1.5){
break;
}
if (analog(1)>500) {
motor(0,50);
motor(2,80);
}else if (analog(3)>500) {
motor(0,80);
motor(2,50);
}else {
motor(0,80); //why is this 60 and 80
motor(2,60);
}
}
//line follow
start=seconds ();
while(1){
if(seconds()-start>9.1){
break;
}
if (analog(1)>500) {
motor(0,80);
motor(2,100);
}else if (analog(3)>500) {
motor(0,100);
motor(2,80);
}else {
motor(0,100);
motor(2,100);
}
}
set_servo_position(3,ARM_DOWN);
go_for(100, 100, 3500); // Ram into PVC at the end of the cave
#if 1
go_for(-50, -90, 1000); // Back up with poms
go_for(90, -90, 1000);
set_servo_position(3, ARM_DOWNISH);
// TODO: Run back across the cave to the good end
start=seconds ();
while(1){
if(seconds()-start>1.5){
break;
}
if (analog(1)>500) {
motor(0,50);
motor(2,80);
}else if (analog(3)>500) {
motor(0,80);
motor(2,50);
}else {
motor(0,80); //why is this 60 and 80
motor(2,60);
}
}
start=seconds ();
while(1){
if(seconds()-start>8.5){ // change from 8 sec
break;
}
if (analog(1)>500) {
motor(0,80);
motor(2,100);
}else if (analog(3)>500) {
motor(0,100);
motor(2,80);
}else {
motor(0,100);
motor(2,100);
}
}
set_servo_position(3, ARM_DOWN);
go(75,75);
msleep(2500);
go_for(-50,-90, 1000); // back up/turn/pull pom-poms
go_for(90, -90, 500);
go_for(-100, -90, 600);
set_servo_position(3, ARM_UP);
go_for(100, 100, 2000);
go_for(-100, -100,2000);
set_servo_position(3, ARM_DOWN);
msleep(700);
go_for(100,100,1000);
go(-10, -10);
set_servo_position(3, ARM_DOWN);
msleep(100);
set_servo_position(3, ARM_DOWN + 50);
msleep(100);
set_servo_position(3, ARM_DOWN + 100);
msleep(100);
set_servo_position(3, ARM_DOWN + 150);
msleep(100);
set_servo_position(3, ARM_DOWN + 200);
msleep(100);
//set_servo_position(3, ARM_DOWN + 250);
//msleep(100);
//set_servo_position(3, ARM_DOWN + 400);
//msleep(100);
//push the poms over the top of the PVC
go_for(20, 20, 1000);
//StOp
go(0,0);
//wiggle the arm up and down so the poms fall off
set_servo_position(3, ARM_DOWN+350);
msleep(250);
set_servo_position(3, ARM_DOWN+300);
msleep(250);
set_servo_position(3, ARM_DOWN+350);
msleep(250);
//backs up to push poms second timeb
set_servo_position(3, ARM_UP);
go_for(-100, -100,2000);
//push poms second time
go_for(100, 100, 2000);
go_for(-100, -100,2000);
set_servo_position(3, ARM_DOWN);
msleep(700);
go_for(100,100,1000);
go(-10, -10);
set_servo_position(3, ARM_DOWN);
msleep(100);
set_servo_position(3, ARM_DOWN + 50);
msleep(100);
set_servo_position(3, ARM_DOWN + 100);
msleep(100);
set_servo_position(3, ARM_DOWN + 150);
msleep(100);
set_servo_position(3, ARM_DOWN + 200);
msleep(100);
//set_servo_position(3, ARM_DOWN + 250);
//msleep(100);
//set_servo_position(3, ARM_DOWN + 400);
//msleep(100);
//push the poms over the top of the PVC
go_for(20, 20, 1000);
//StOp
go(0,0);
//wiggle the arm up and down so the poms fall off
set_servo_position(3, ARM_DOWN+350);
msleep(250);
set_servo_position(3, ARM_DOWN+300);
msleep(250);
set_servo_position(3, ARM_DOWN+350);
msleep(250);
//motor(0,-75);
//motor(2,-75);
//msleep(8500);
//turn_left();
//go_for(100, 100, 1800);
//turn_left();
//go_for(100, 100, 5000);
#endif
ao();
return 0;
}
int main()
{
double start = 0.0;
// initialize servo, camera, etc here?.
// if you are using a Create you should uncomment the next line
//create_connect();
enable_servos ();
set_servo_position(3, ARM_UP);
int a1;
int a2;
//goto START_DOWN_CAVE;
//lights
#if 0
wait_for_light(2); // change the port number to match where your robot
shut_down_in(119); // shut off the motors and stop the Create after 119 seconds
// note that shut_down_in should immediately follow wait_for_light!!
printf("My program should score more points than this template!\n");
// replace printf above with your code
#endif
//create_disconnect();
//go forward to line follow
motor(0,700);
motor(2,700);
msleep(1000);
while ((a1 = analog(1)) <500 && (a2 = analog(3)) <500) {
printf("%5d %5d\n",a1,a2);
//msleep(5);
if(seconds()-start>6.5){
printf("time_out\n");
break;
}
}
printf("Found black %d\n", a1);
//
//line follow
start=seconds ();
while(1){
a1 =analog (1);
a2 =analog (3);
printf("cave%5d %5d\n",a1,a2);
if(seconds()-start>4.5){
break;
}
if ((a1<500 && a2<500) || (a1>500 && a2>500)) {
motor(0,90 * factor);
motor(2,56 * factor);
} else if (a1<500) {
motor(0,90 * 1.00 * factor);
motor(2,75 * 0.80 * factor);
}else {
motor(0,90 * 0.80 * factor);
motor(2,75 * 1.00 * factor);
}
}
return 0;
}
int main()
{
// INITIALIZE
int x, y, color=0, pink=1; // set up for color channel 0 (green)
int xvalue, yvalue;
int greencentered = 0;
int loop = 0;
int centerx=77; // calibrate these number and enter manually
int centery=63;
int margin=8;
int deltax = 0;
int deltay = 0;
int stepsizex;
int stepsizey;
int currpos;
int s_time;
light_start(L_SENSOR); // light start
shut_down_in(119);
camera_open(LOW_RES);
enable_servos(); // enable servos
set_servo_position(S_CATCHER,S_DOWN);
msleep(500);
set_servo_position(S_GATE,S_OPEN);
msleep(500);
// TRIBBLE PILE 1
//set_servo_position(S_GATE,S_OPEN); // open gate
msleep(5000);
set_servo_position(S_CATCHER,CATCHER_UP);
right(28,ks/2); // right
f_until_black(TOPHAT_RIGHT/*,TOPHAT_LEFT*/); // forward until right sensor sees black]
forward(2);
//printf("see black! time to close the gate\n");
set_servo_position(S_GATE,S_GAP); // close gate
//f_until_white(TOPHAT_RIGHT);
set_servo_position(CATCHERARM, CATCHER_UP);
// camera sort
s_time = curr_time();
while((curr_time()-s_time) < 14)
{
while (greencentered == 0)
{
camera_update(); // process the most recent image
camera_update(); // process the most recent image
camera_update(); // process the most recent image
camera_update(); // process the most recent image
camera_update(); // process the most recent image
loop = loop + 1;
printf("Update camera, loop %d\n", loop);
//msleep(10);
if (get_object_count(color) > 0)
{
xvalue = get_object_center(color,0).x;
yvalue = get_object_center(color,0).y;
deltax = abs(xvalue-centerx);
deltay = abs(yvalue-centery);
stepsizex = (deltax>10) ? 50: 20;
stepsizey = (deltay>10) ? 50: 20;
printf("x is %d, y is %d\n", xvalue, yvalue);
greencentered = ((xvalue >= centerx-margin) & (xvalue <= centerx+margin)) && ((yvalue >= centery-margin) & (yvalue <= centery+margin));
if(greencentered == 1)//get x, y for the biggest blob the channel sees
{
printf("Biggest blob at (%d,%d)\n",xvalue,yvalue);
msleep(300);
set_servo_position(CATCHERARM, CATCHER_MIDWAY);
msleep(300);
set_servo_position(CATCHERARM, CATCHER_DOWN);
//slow_servo(CATCHERARM,20,40,CATCHER_UP,CATCHER_DOWN);
msleep(300);
currpos=get_servo_position(CATCHERARM);
printf("%d\n",currpos);
/*if(CATCHER_MIDWAY>=currpos>CATCHER_DOWN)
{
currpos=get_servo_position(CATCHERARM);
set_servo_position(CATCHERARM, currpos-INCREMENT);
msleep(SLEEP_INCREMENT);
currpos=get_servo_position(CATCHERARM);
}
if(currpos<CATCHER_MIDWAY)
{
currpos=get_servo_position(CATCHERARM);
set_servo_position(CATCHERARM, currpos+INCREMENT);
msleep(SLEEP_INCREMENT);
currpos=get_servo_position(CATCHERARM);
}*/
set_servo_position(CATCHERARM,CATCHER_UP);
msleep(500);
}
else
{
if(xvalue <= centerx+margin) //moves right if senses value greater than 80
{
motor(MOT_RIGHT,70);
motor(MOT_LEFT,-70);
msleep(stepsizex);
motor(MOT_RIGHT,0);
motor(MOT_LEFT,0);
//msleep(10);
printf("blob is too right\n");
}
if(xvalue >= centerx-margin) // moves left if senses value less than 70
{
motor(MOT_LEFT,70);
motor(MOT_RIGHT,-70);
msleep(stepsizex);
motor(MOT_LEFT,0);
motor(MOT_RIGHT,0);
//msleep(10);
printf("blob is too left\n");
}
if ((xvalue > 67 /*centerx-margin*/) && (xvalue < 87 /*centerx+margin*/))
{
if(yvalue > centery+margin) //moves forward if senses value greater than 43
{
motor(MOT_LEFT,-50);
motor(MOT_RIGHT,-50);
msleep(stepsizey);
motor(MOT_LEFT,0);
motor(MOT_RIGHT,0);
//msleep(10);
printf("blob is too close\n");
}
if(yvalue < centery-margin) // moves backwar if senses value less than 38
{
motor(MOT_LEFT,50);
motor(MOT_RIGHT,50);
msleep(stepsizey);
motor(MOT_LEFT,0);
motor(MOT_RIGHT,0);
//msleep(10);
printf("blob is too far\n");
}
}
}
}
}
}
set_servo_position(S_GATE,S_CLOSE);
while (analog10(2)<BLACK_SEN_THRESH)
{
motor(MOT_LEFT,-100);
}
// SCORE PILE 1
left(5,0);
//backward(50);
touch_back(TOUCH_SEN);
forward(10);
backward(10);
forward(20);
right(55,0);
forward(35);
backward(4);
right(45,0);
while (analog10(3)<BLACK_SEN_THRESH)
{
motor(MOT_LEFT, 100);
motor(MOT_RIGHT, 100);
}
msleep(10);
backward(24);
left(42,0);
backward(10);
left(50,0);
//backward(35);
touch_back(TOUCH_SEN);
forward(120.00);
// TRIBBLE PILE 2
set_servo_position(S_GATE,S_OPEN);
forward(40.00);
set_servo_position(S_GATE,S_GAP);
// camera sort
s_time = curr_time();
while((curr_time()-s_time) < 14) //timer
{
while (greencentered == 0)
{
camera_update(); // process the most recent image
camera_update(); // process the most recent image
camera_update(); // process the most recent image
camera_update(); // process the most recent image
camera_update(); // process the most recent image
loop = loop + 1;
printf("Update camera, loop %d\n", loop);
//msleep(10);
if (get_object_count(color) > 0)
{
xvalue = get_object_center(color,0).x;
yvalue = get_object_center(color,0).y;
deltax = abs(xvalue-centerx);
deltay = abs(yvalue-centery);
stepsizex = (deltax>10) ? 50: 20;
stepsizey = (deltay>10) ? 50: 20;
printf("x is %d, y is %d\n", xvalue, yvalue);
greencentered = ((xvalue >= centerx-margin) & (xvalue <= centerx+margin)) && ((yvalue >= centery-margin) & (yvalue <= centery+margin));
if(greencentered == 1)//get x, y for the biggest blob the channel sees
{
printf("Biggest blob at (%d,%d)\n",xvalue,yvalue);
msleep(300);
set_servo_position(CATCHERARM, CATCHER_MIDWAY);
msleep(300);
set_servo_position(CATCHERARM, CATCHER_DOWN);
//slow_servo(CATCHERARM,20,40,CATCHER_UP,CATCHER_DOWN);
msleep(300);
currpos=get_servo_position(CATCHERARM);
printf("%d\n",currpos);
/*if(CATCHER_MIDWAY>=currpos>CATCHER_DOWN)
{
currpos=get_servo_position(CATCHERARM);
set_servo_position(CATCHERARM, currpos-INCREMENT);
msleep(SLEEP_INCREMENT);
currpos=get_servo_position(CATCHERARM);
}
if(currpos<CATCHER_MIDWAY)
{
currpos=get_servo_position(CATCHERARM);
set_servo_position(CATCHERARM, currpos+INCREMENT);
msleep(SLEEP_INCREMENT);
currpos=get_servo_position(CATCHERARM);
}*/
set_servo_position(CATCHERARM,CATCHER_UP);
msleep(500);
}
else
{
if(xvalue <= centerx+margin) //moves right if senses value greater than 80
{
motor(MOT_RIGHT,70);
motor(MOT_LEFT,-70);
msleep(stepsizex);
motor(MOT_RIGHT,0);
motor(MOT_LEFT,0);
//msleep(10);
printf("blob is too right\n");
}
if(xvalue >= centerx-margin) // moves left if senses value less than 70
{
motor(MOT_LEFT,70);
motor(MOT_RIGHT,-70);
msleep(stepsizex);
motor(MOT_LEFT,0);
motor(MOT_RIGHT,0);
//msleep(10);
printf("blob is too left\n");
}
if ((xvalue > 67 /*centerx-margin*/) && (xvalue < 87 /*centerx+margin*/))
{
if(yvalue > centery+margin) //moves forward if senses value greater than 43
{
motor(MOT_LEFT,-50);
motor(MOT_RIGHT,-50);
msleep(stepsizey);
motor(MOT_LEFT,0);
motor(MOT_RIGHT,0);
//msleep(10);
printf("blob is too close\n");
}
if(yvalue < centery-margin) // moves backwar if senses value less than 38
{
motor(MOT_LEFT,50);
motor(MOT_RIGHT,50);
msleep(stepsizey);
motor(MOT_LEFT,0);
motor(MOT_RIGHT,0);
//msleep(10);
printf("blob is too far\n");
}
}
}
}
}
}
set_servo_position(S_GATE,S_CLOSE);
// SCORE PILE 2
right(55,0);
forward(28);
backward(12);
right(55,0);
touch_back(TOUCH_SEN);
forward(200);
right(50,0);
touch_back(TOUCH_SEN);
forward(15);
left(50,0);
while (analog10(3)<BLACK_SEN_THRESH)
{
motor(MOT_LEFT, 100);
motor(MOT_RIGHT, 100);
}
backward(4);
right(50,0);
msleep(10);
forward(24);
right(42,0);
touch_back(TOUCH_SEN);
backward(4);
// END
ao();
disable_servos();
printf("done!");
}
int main() {
//light_start(0);
shut_down_in(119.5);
arm_up();
claw_open();
bin_down();
enable_servos();
collect_poms();
printf("Collected Initial Poms in base\n");
/*
forward(70);
msleep(500);
left(35, 0);
msleep(300);
//face 1st pile
forward(45);
pomPileOne();
*/
forward(47);
msleep(500);
left(30, 0);
//face 1st pile
forward(60); //needs to be exact or else claw will hit table divider
pomPileOne();
/*
//4/9/16 Test Code
forward(20);
msleep(500);
left(15, 0);
msleep(300);
//face 1st pile
forward(80);
pomPileOne();
*/
//go to pile 2
multforward(45, 0.50);
msleep(100);
forward(15);
msleep(300);
left(85, ks/2);
msleep(300);
forward(10);
pomPileTwo();
if(green == 1 && red == 1) {
printf("Collected all\n");
//go to bin
right(60, ks/2);
forward(100);
right(100, ks/2);
msleep(500);
bin_mid();
msleep(500);
bin_dump();
}
else {
//do pom pile three only if robot does not have both one red and one green
pomPileThree();
//go to bin
left(60, ks/2);
forward(60);
}
msleep(2000);
disable_servos();
ao();
return 0;
}
int main()
{
printf("version 1.9.5\n");
shut_down_in(0.010*1000);
int start=seconds();//starting time for two minutes
enable_servo(0);
set_servo_position(0,2047);
straight(1.8, 170);//gets in front of transport
rightC(0.28, 100);
leftC(0.28, -100);
straight(1.65, 170);
leftC(0.44, 100);
printf("completed first dead reckoning\n");
set_analog_pullup(ETport, 0);
//until distance is 5 centimeter
/*
int claw_threshold = 13;
while(analog10(ETport)>claw_threshold){
back(0.1, -10);
msleep(1000);
printf("move once\n");
printf("%i\n", analog10(ETport));
}*/
back(0.25, -20);//pushes against transport
msleep(2000);
set_servo_position(0, 500);//puts claw on transport
msleep(1000);
printf("done with attaching to transport\n");
while(digital(8)==0)//goes forward with transport until it hits the wall
{
straight(0.1, 100);
msleep(10);
}
printf("at wall\n");
back(0.25, -50);//backs up from wall to deposit transport
set_servo_position(0, 2047);//raises claw
msleep(1500);
while(digital(8)==0){//goes forward until it hits wall for second time
straight(0.1, 100);
msleep(10);
}
printf("at wall 2\n");
back(0.35, -100);//goes back and turns left in order to be oriented with left inner wall
leftC(0.6, 142);
int bump_counter=0;
while(digital(8)==0){//hits side wall
straight(0.1, 100);
msleep(10);
}
bump_counter++;//has one bump
int i=0;
while(i<10){//if hits side wall, pause
stop(0.2);
i++;
}
back(0.25, -100);
leftC(0.46, 100);
camera_open(LOW_RES);
armUp();
clawOpen();
double start_wait=seconds();
while((seconds()-start_wait)<=25){//wait for poms or for 25 seconds
int i=0;
while(i<10){//picks latest image from the buffer
camera_update();
i++;
}
if(get_object_count(chan)>0){//if camera sees objects, skip the 25 seconds and go onto picking up stuff
break;
}else{//if camera doesn't see any objects, keep waiting until 25 seconds is up
stop(0.1);
}
}
int x=get_object_center(chan, 0).x;//declares global unchanging variable for the x location of the largest object
while((seconds()-start)<=120){//while there is still time left
int area=get_object_area(chan, 0);//creates local changing variable. this is th area of the largest object camera sees
if(area<=600){//if the object is small enough(far enough), navigate towards object. 600 is threshold
int i=0;
while(i<10){//buffer updating
camera_update();
i++;
}
navigate_poms(x);//similar to line followig function; gets to poms.
}
armDown();
clawClose();
armUp();
/*
leftF(0.5, 100, 80);//turn and go forward until transport is contacted
while(digital(8)==0){
straight(0.1, 100);*/
}
/*
back(1.5, 100);
leftC(0.44, 100);
while(digital(8)==0 && digital(9)==1){
straight(0.1, 100);
msleep(10);
if(digital(9)==0){
rightF(0.1, 100, 40);
left(0.1, 100);
msleep(10);
}
}
straight(2, 200);
rightC(0.44, 100);
straight(1, 80);
rightC(0.92, 100);
while(analog10(ETport<=600)){
msleep(10);
}
msleep(10000);
straight(1, 80);
leftC(0.44, 100);
straight(2, 200);
*/
return 0;
}
int main()
{
enable_servos();
printf("Houston we have ignition\n");
set_servo_position(0, 950);
set_servo_position(1, 60);
msleep (1000);
while (analog(0) > 700 && a_button() == 0)
{
msleep(10);
}
shut_down_in(118.0);
msleep(2000);
go(40, 0);
msleep(3000);
stop();
set_servo_position(0, 1350);
msleep(1000);
set_servo_position(1, 1400);
msleep(1000);
set_servo_position(0,600);
stop();
msleep(1000);
go (50,50);
msleep (4900);
stop ();
set_servo_position(0, 498);
msleep(2500);
go (70, 40);
msleep(2600);
stop ();
go (35, -30);
msleep(1000);
go (15, 15);
msleep(1500);
stop ();
set_servo_position(1, 600);
go (-40, -40);
msleep(4000);
stop();
go (-35, 30);
msleep(1650);
stop();
go (40, 40);
msleep (1950);
set_servo_position(1, 600);
return 0;
}
int main(int argc, char** argv) {
enable_servo(kServoPortSorter);
set_servo_position(kServoPortSorter, kServoPositionSorterCenter);
clear_motor_position_counter(kMotorPortBayLeft);
clear_motor_position_counter(kMotorPortBayRight);
create_connect();
camera_open(LOW_RES);
scanf("%s", NULL);
printf("waiting for light\n");
while (analog(0) > 200) {}
shut_down_in(700000);
motor(kMotorPortBayLeft, 10);
motor(kMotorPortBayRight, 10);
while (get_motor_position_counter(kMotorPortBayLeft) < 230 || get_motor_position_counter(kMotorPortBayRight) < 230) {
printf("pos: %i/%i\n", get_motor_position_counter(kMotorPortBayLeft), get_motor_position_counter(kMotorPortBayRight));
}
motor(kMotorPortBayLeft, 0);
motor(kMotorPortBayLeft, 0);
// wait_for_side_button();
msleep(6000);
create_drive_straight(200);
msleep(1850);
create_spin_CCW(200);
msleep(750);
create_drive_straight(-200);
msleep(1500);
create_spin_CW(200);
msleep(1750);
create_drive_straight(200);
while (!get_create_lbump() && !get_create_rbump()) {}
create_drive_straight(-150);
msleep(900);
create_spin_CCW(200);
msleep(950);
create_drive_straight(150);
while (!get_create_lbump() && !get_create_rbump()) {}
create_stop();
msleep(10000);
thread all_off = thread_create(wait_for_kill);
thread_start(all_off);
raise_bay();
create_spin_CW(100);
msleep(500);
create_drive_straight(-100);
msleep(1000);
create_stop();
thread jiggle_c = thread_create(jiggle_create);
thread_start(jiggle_c);
thread sort_b = thread_create(sort_balls);
thread_start(sort_b);
msleep(33000);
while (true) {}
/*thread_destroy(jiggle_c);
create_stop();
lower_bay();
create_drive_straight(100);
while (!get_create_lbump() && !get_create_rbump()) {}
create_stop();
msleep(10000);
raise_bay();
create_spin_CW(100);
msleep(500);
create_drive_straight(-100);
msleep(1000);
create_stop();
thread_create(jiggle_create);
thread_start(jiggle_c);
thread_create(sort_balls);
while (true) {}
camera_close();*/
return 0;
}
int main()
{
shut_down_in(30);
countdown();
}
int main()
{
//waiting for light to start the program.
wait_for_light(0);
shut_down_in(120.0);
//making sure the claw and arm are in position.
set_servo_position(1, 0);//ensures that the arm is in the right position
sleep(0.6);//0.6 is how many seconds the motors are turning on their input values
set_servo_position(0, 624);//ensures that the claw is in the right position
sleep(0.6);//0.6 is how many seconds the motors are turning on their input values
//this section allows the robot to come out of the starting box, turn, drive forward and put itself in position to grab the polyps.
mav(0, -1000);//-600 is velocity
mav(2, -975);//-600 is velocity
sleep(5.0);//7.9 is how many seconds the motors are turning on their input values
mav(0, -500);//-500 is velocity
mav(2, 500);//500 is velocity
sleep(1.6);//7.9 is how many seconds the motors are turning on their input values
mav(0, 900);
mav(2, 900);
sleep(1.8);
mav(2, -800);//-800 is the velocity
mav(0, -800);//-800 is the velocity
sleep(2.0);//1.8 is how many seconds the motors are turning on their input values
mav(0, -300);//-300 is the velocity
mav(2, -100);//-100 is the velocity
sleep(1.9);//1.7 is how many seconds the motors are turning on their input values
ao();//turns motors off
//in this ectionthe arm is lowered the claw closes capturing the polyps and the arm is raised to expose the touch sensor.
set_servo_position(0, 800);//this lowers the claw
sleep(0.6);//0.6 is how many seconds it give the servo to spin to the input position
set_servo_position(0, 900);
sleep(1.8);
set_servo_position(0, 1000);
sleep(1.6);
set_servo_position(0, 1300);
sleep(2.6);
set_servo_position(0, 1700);//this lowers the claw all the way to the board
sleep(1.4);//1.4 is how many seconds it give the servo to spin to the input position
set_servo_position(1, 2048);//this closes the claws with the polyps inside
sleep(0.6);//0.6 is how many seconds it give the servo to spin to the input position
set_servo_position(0, 1324);//this raises the arm to expose the touch sensors
sleep(0.6);//0.6 is how many seconds it give the servo to spin to the input position
//in this section the robot turns toward MPA and drives straight until the touch sensor is triggered by hitting one of the walls of the MPA.
mav(0, -100);
mav(2, -300);
sleep(2.0);
mav(0, 1000);
mav(2, 1000);
sleep(2.8);
mav(0, -200);
mav(2, -200);
sleep(2.6);
mav(0, 500);
mav(2, -500);
sleep(1.0);
set_servo_position(0, 1324);
sleep(0.6);
while(digital(8)==0)
{
mav(2, -900);
mav(0, -700);
sleep(0.2);
if(digital(9)==1)
{
mav(2, -700);
mav(0, -975);
sleep(0.7);
}
}
mav(0, 300);
mav(2, 300);
sleep(1.2);
ao();
set_servo_position(0, 1224);
sleep(0.8);
set_servo_position(1, 0);
sleep(1.4);
set_servo_position(0, 1024);
sleep(0.2);
set_servo_position(0, 1124);
sleep(0.2);
set_servo_position(1, 2048);
sleep(1.4);
set_servo_position(1, 0);
sleep(1.4);
set_servo_position(1, 2048);
sleep(1.4);
set_servo_position(0, 524);
sleep(0.6);
set_servo_position(1, 0);
sleep(1.4);
set_servo_position(0, 1424);
sleep(0.6);
set_servo_position(1, 0);
sleep(1.4);
set_servo_position(0, 624);
sleep(0.6);
set_servo_position(0, 1424);
sleep(0.8);
set_servo_position(0, 624);
sleep(0.8);
mav(0, 300);
mav(2, 0);
sleep(0.6);
mav(0, 500);
mav(2, 500);
sleep(0.8);
mav(0, 500);
mav(2, 500);
sleep(0.1);
mav(0, -500);
mav(2, 500);
sleep(1.8);
mav(0, 500);
mav(2, -500);
sleep(1.8);
mav(0, -500);
mav(2, -500);
sleep(0.1);
mav(0, -500);
mav(2, 500);
sleep(1.6);
mav(0, 1000);
mav(2, 1000);
sleep(1.0);
set_servo_position(0, 1324);
sleep(1.0);
mav(0, 1000);
mav(2, 1000);
sleep(1.8);
mav(0, -500);
mav(2, -500);
sleep(4.6);
mav(0, -500);
mav(2, 500);
sleep(1.7);
mav(0, -500);
mav(2, -500);
sleep(2.0);
mav(0, -500);
mav(2, 500);
sleep(1.6);
//mav(0, 500);
//mav(3, 500);
//sleep(0.8);
mav(0, 300);
mav(2, 300);
sleep(0.2);
ao();
set_servo_position(0, 1024);
sleep(1.4);
set_servo_position(1, 700);
sleep(16.0);
set_servo_position(1, 0);
set_servo_position(0, 1324);
sleep(1.4);
sleep(1.0);
mav(0, 500);
mav(2, 500);
sleep(1.2);
mav(0, 500);
mav(2, -500);
sleep(1.6);
mav(0, -1000);
mav(2, -1000);
sleep(2.3);
mav(0, 500);
mav(2, -500);
sleep(1.4);
while(digital(8)==0)
{
mav(2, -1975);
mav(0, -2000);
sleep(0.5);
}
mav(0, 500);
mav(2, -500);
sleep(1.6);
mav(2, -1975);
mav(0, -2000);
sleep(2.3);
}
void startup()
{
shut_down_in(115);//We get 120 seconds, but its nice to be on the safe side
//if using this, make sure to use ao()
}
int main(int argc, char** argv) {
startup_servos();
//wait_for_light(0);
scanf("%s", NULL);
printf("waiting for light\n");
while (analog(0) > 200) {}
shut_down_in(700000);
raise_scoop_level();
drive_straight(900);
drive_straight(-50);//old value 320
/*//to be replaced with tick thing
motor(kMotorPortDriveLeft, 100);
motor(kMotorPortDriveRight, 5);
msleep(2430);
motor(kMotorPortDriveLeft, 0);
motor(kMotorPortDriveRight, 0);*/
//dump();
//close_scoop_doors();
//open_scoop_doors();
//dump();
//msleep(800);
turn_ticks_in_place(50, 890);
// pile 1
open_scoop_doors();
lower_scoop_level();
drive_straight(250);
close_scoop_doors();
raise_scoop_level();
//Reverse to pile 2
drive_straight(-290);
turn_ticks(50, -700);
drive_straight(-250);
//set_servo_position(kServoPortScoopTiltLeft, kServoPositionScoopTiltHighBack);
//set_servo_position(kServoPortScoopTiltRight, 2047 - kServoPositionScoopTiltHighBack);
turn_ticks(50, 770);//this value
lower_scoop_level();
open_scoop_doors();
//pick up pile 2
drive_straight(270);
close_scoop_doors();
raise_scoop_level();
drive_straight(80);
raise_scoop_very_high();
drive_straight(170);
turn_ticks(50, 500);
drive_straight(-90);
turn_ticks_in_place(50, 650);
drive_straight(100);
//set_servo_position(kServoPortScoopTiltLeft, kServoPositionScoopTiltHigh - 300);
//set_servo_position(kServoPortScoopTiltLeft, (2047 - kServoPositionScoopTiltHigh) + 300);
// turn_ticks(50, 555);
// drive_straight(50);
//set_servo_position(kServoPortScoopTiltLeft, 1000);
//set_servo_position(kServoPortScoopTiltRight, 2047 - 1000);
msleep(200);
open_doors_wide();
dump();
msleep(500);
dump_shake();
dump();
dump_shake();
dump();
//raise_scoop();
drive_straight(-400);
return 0;
raise_scoop_level();
close_doors_wide();
raise_scoop_very_high();
turn_ticks_in_place(50, -1350);
drive_straight(100);
//drive to pile 3
raise_scoop_level();
turn_ticks_in_place(50, 700);
turn_ticks_in_place(50, -400);
lower_scoop_level();
turn_ticks_in_place(50, -300);
drive_straight(100);
//pick up pile 3
open_scoop_doors();
drive_straight(400);
close_scoop_doors();
turn_ticks_in_place(50, -100);
drive_straight(-400);
raise_scoop_level();
turn_ticks_in_place(50, -100);
drive_straight(-400);
turn_ticks_in_place(50, -200);
drive_straight(-290);
lower_scoop_level();
turn_ticks_in_place(50, 1150);
turn_ticks_in_place(50, -100);
raise_scoop_level();
// drive to/pick up pile 4
drive_straight(500);
turn_ticks_in_place(50, 400);
mid_scoop_level();
turn_ticks_in_place(50, -900);
raise_scoop_level();
turn_ticks_in_place(50, 400);
lower_scoop_level();
turn_ticks_in_place(50, 500);
turn_ticks_in_place(50, -50);
open_scoop_doors();
drive_straight(230);
close_scoop_doors();
raise_scoop_level();
// dump other load
turn_ticks_inverse(50, 500);
drive_straight(-100);
turn_ticks_in_place(50, -100);
drive_straight(-150);
raise_scoop_very_high();
turn_ticks_in_place(50, 700);
drive_straight(200);
set_servo_position(kServoPortScoopTiltLeft, 1000);
set_servo_position(kServoPortScoopTiltRight, 2047 - 1000);
msleep(200);
open_doors_wide();
dump();
msleep(500);
dump_shake();
dump();
dump_shake();
dump();
return 0;
turn_ticks_in_place(50, -475);
drive_straight(-200);
turn_ticks_in_place(50, 475);
drive_straight(400);
//pick up pile 4
lower_scoop_level();
drive_straight(500);
open_scoop_doors();
}
int main()
{
//wait_for_light(1);//initialization
printf("version 1.9.5\n");
shut_down_in(115);
int servo_counter=0;
while(servo_counter<=3){
enable_servo(servo_counter);
servo_counter++;
}
// initialize routine
set_servo_position(0,2047);
armUp();
clawClose();
//get in front of transport
straight(1.8, 170);
rightC(0.28, 100);
leftC(0.28, -100);
straight(1.65, 170);
leftC(0.44, 100);
printf("completed first dead reckoning\n");
//until distance is 5 centimeter
/*
int claw_threshold = 13;
while(analog10(ETport)>claw_threshold){
back(0.1, -10);
msleep(1000);
printf("move once\n");
printf("%i\n", analog10(ETport));
}*/
back(0.25, -20);//pushes against transport
msleep(2000);
set_servo_position(0, 500);//puts claw on transport
msleep(1000);
printf("done with attaching to transport\n");
while(digital(8)==0)//goes forward with transport until it hits the wall
{
straight(0.1, 100);
msleep(10);
}
printf("at wall\n");
back(0.15, -50);//backs up from wall to deposit transport
set_servo_position(0, 2047);//raises claw
msleep(1500);
while(digital(8)==0){//goes forward until it hits wall for second time
straight(0.1, 100);
msleep(10);
}
printf("at wall 2\n");
back(0.35, -100);//goes back and turns left in order to be oriented with left inner wall
leftC(0.6, 142);
int bump_counter=0;
while(digital(8)==0){//hits side wall
straight(0.1, 100);
msleep(10);
}
bump_counter++;//has one bump
int i=0;
while(i<10){//if hits side wall, pause
stop(0.2);
i++;
}
/*
==========================
==========End of routine 1======
KIPR at the wall
==========================
*/
// Pause here to wait for Create to drop POMS
msleep(1*1000);
//
int start=seconds();// start time
printf("will get out of corner\n");
back(0.5, -100);
while(seconds()-start<=3){//separates back from right angle
stop(0.1);
}
right_angle(left);
while(seconds()-start<=5){//separates right angle from straight
stop(0.1);
}
double start_drive=seconds();
straight(0.7, 100);//go to poms
stop(0.5);
camera_open(LOW_RES);
/*while(seconds()-start<=20){
int update_counter=0;
while(update_counter<10){
camera_update();
update_counter++;
}
if(get_object_count(chan)==0){
stop(0.1);
}
int x=get_object_center(chan, 0).x;
int area=get_object_area(chan, 0);
if(area<=400){
navigate(x);
}
if(area>=800){
//if objects are clumped are clumped, area will be automatically larger.
//this means that KIPR will start earlier than it should. We counter this here
//straight(0.18, 100);
straight(0.1, 100);
break;
}
}*/
stop(0.1);
printf("object seen\n");
double end_drive=seconds();//only for simulation. real end and start times would have to be taken from whole routine
//pick up POMS that are right there
pickup();
int offset=(end_drive-start_drive)*(3.0/4.0);;//half of distance from back of launch area to border of launch area
back((end_drive-start_drive)+1.5, -100);//go to the back of wall plus a little more to straighten out
printf("at back wall\n");
straight(2.0, 100);//goes halway between border and back of launch area
right_angle(left);//faces transport
int starpof=seconds();//time after right angle
while(seconds()-starpof<=2){
motor(motorL, 0);
motor(motorR, 0);
}
while(digital(8)==0){
straight(0.1, 100);
}
straight(0.1, 100);
printf("touching transport\n");
motor(motorL, 0);
motor(motorR, 0);
dropoff();//drops poms in launch area
printf("poms in transport\n");
back(0.25, -100);
int starfop=seconds();
while(seconds()-starfop<=2){
motor(motorL, 0);
motor(motorR, 0);
}
int turns=1;
while(turns<=2){
right_angle(right);
stop(0.3);
turns++;
}
/*
======================
======Part 2 of routine 2====
======================
*/
while(1){
stop(0.5);//
while(digital(8)==0){
straight(0.1, 100);
}
back(0.3, -100);
right_angle(left);
pickup();
back((end_drive-start_drive)+1.5, -100);
straight(2.2, 100);
right_angle(left);
while(digital(8)==0){
straight(0.1, 100);
}
dropoff();
back(0.5, -100);
right_angle(left);
right_angle(left);
}
/*while(1){
start=0;
while(digital(8)==0){
straight(0.1, 80);
}
back(0.5, -100);
right_angle(right);
straight(0.5, 100);
right_angle(right);
right_angle(right);
while(seconds()-start<=20){
int update_counter=0;
while(update_counter<10){
camera_update();
update_counter++;
}
if(get_object_count(chan)==0){
stop(0.1);
}
int x=get_object_center(chan, 0).x;
int area=get_object_area(chan, 0);
if(area<=400){
navigate(x);
}
if(area>=800){
//if objects are clumped are clumped, area will be automatically larger.
//this means that KIPR will start earlier than it should. We counter this here
//straight(0.18, 100);
straight(0.1, 100);
break;
}
}
int servo_counter=1;
while(servo_counter<=3){
enable_servo(servo_counter);
printf("servo %d enabled\n", servo_counter);
servo_counter++;
}
pickup();
int offset=(end_drive-start_drive)*(3.0/4.0);;//half of distance from back of launch area to border of launch area
back((end_drive-start_drive)+1.5, -100);//go to the back of wall plus a little more to straighten out
printf("at back wall\n");
straight(1.8, 100);//goes halway between border and back of launch area
right_angle(left);//faces transport
int starpof=seconds();//time after right angle
while(seconds()-starpof<=2){
motor(motorL, 0);
motor(motorR, 0);
}
while(digital(8)==0){
straight(0.1, 100);
}
straight(0.1, 100);
printf("touching transport\n");
motor(motorL, 0);
motor(motorR, 0);
dropoff();//drops poms in launch area
printf("poms in transport\n");
back(0.25, -100);
int starfop=seconds();
while(seconds()-starfop<=2){
motor(motorL, 0);
motor(motorR, 0);
}
int turns=1;
while(turns<=2){
right_angle(right);
turns++;
}
repeat_runs++;
}*/
while(1){
int servo_counter=0;//enables servos
while(servo_counter<=3){
enable_servo(servo_counter);
servo_counter++;
}
armUp();//raises arm at start
stop(0.5);
while(digital(TOUCH_SENSOR)==0){//go forward until left wall is contacted
straight(0.1, 100);
}
stop(0.1);
back(0.3, -100);//back up
stop(0.1);
right_angle2(left);//face poms
stop(0.1);
back(3+1.5, -100);//back up to rocket wall to straighten out
stop(0.1);
straight(3+1.5, 100);//go to poms
stop(0.1);
pickup();//pick poms up
stop(0.1);
back(3+1.5, -100);// back up until rocket wall
stop(0.1);
straight(2.2, 100);//go forward
stop(0.1);
right_angle2(left);//turn to face transport
stop(0.1);
while(digital(TOUCH_SENSOR)==0){//go forward until transport contacted
straight(0.1, 100);
}
stop(0.1);
dropoff();//drop poms off
stop(0.1);
back(0.5, -100);//back up from transpor
stop(0.1);
right_angle2(right);//orient for next iteration
stop(0.1);
right_angle2(right);
}
return 0;
}
int main(int argc, char *argv[])
{
shut_down_in(10.0);
for(;;);
return 0;
}
int main() {
light_start(0);
shut_down_in(119.5);
arm_up();
claw_open();
bin_down();
enable_servos();
collect_poms();
printf("Collected Initial Poms in base\n");
/*
forward(70);
msleep(500);
left(35, 0);
msleep(300);
//face 1st pile
forward(45);
pomPileOne();
*/
forward(50);
msleep(500);
left(28, 0);
//face 1st pile
forward(59); //needs to be exact or else claw will hit table divider
msleep(500);
pomPileOne();
/*
//4/9/16 Test Code
forward(20);
msleep(500);
left(15, 0);
msleep(300);
//face 1st pile
forward(80);
pomPileOne();
*/
//go to pile 2
multforward(45, 0.50);
msleep(100);
forward(15);
msleep(300);
left(85, ks/2);
msleep(300);
backward(2);
//forward(2);
pomPileTwo();
/*
if(green == 1 && red == 1) {
printf("Collected all\n");
//go to bin
right(60, ks/2);
forward(100);
right(100, ks/2);
msleep(500);
bin_mid();
msleep(500);
bin_dump();
}
else {
//do pom pile three only if robot does not have both one red and one green
pomPileThree();
//go to bin
left(60, ks/2);
forward(60);
}
*/
backward(5);
right(80, ks/2);
msleep(300);
forward(130);
msleep(300);
right(50, ks/2); //turn against the wall
msleep(300);
backward(5);
msleep(500);
/*
arm_mid();
msleep(300);
bin_mid();
msleep(500);
bin_dump();
msleep(1000);
*/
right(40, ks/2);
msleep(500);
backward(15);
servo(MAIN_ARM, 1200); //move away arm from box
msleep(300);
bin_mid();
msleep(500);
bin_dump();
msleep(1000);
bin_mid();
msleep(500);
bin_dump();
msleep(1000);
/*
int i = 0;
for(i = 0; i < 3; i++) {
forward(2);
msleep(300);
backward(2);
msleep(300);
}
*/
msleep(1000);
right(30, ks/2);
forward(90);
msleep(2000);
disable_servos();
ao();
return 0;
}
int main()
{
printf("checklist");
printf("hit black button when ready");
while(black_button());
printf("MAKE SURE CREATE IS ON!!!!!!!!");
while(!black_button());
printf("MAKE SURE ARM IS IN DOWN POSITION!!!!!!!!");
while(!black_button());
printf("MAKE SURE ARM AND CLAW WORK");
while(!black_button());
printf("CHECK POSITION OF CLAW AND ARM");
while(!black_button());
printf("MAKE SURE LIGHT SENSORT IS CALIBRATED");
while(!black_button());
// this starts the robot SECTion 1
wait_for_light(0);
shut_down_in(120.0);
//this shuts the robot down after 120.0 seconds
while(create_connect());
//connects create to cbc
set_servo_position(3, 0);
set_servo_position(0, 1800);
enable_servos();
msleep(500);
create_full();
//section 2
create_drive_direct(SUPER_FAST, SUPER_FAST);
msleep(1646);
//leave beach
create_stop();
msleep(300);
create_drive_direct(SUPER_FAST, -SUPER_FAST);
msleep(435);
//turn towards center
create_stop();
msleep(300);
create_drive_direct(SUPER_FAST, SUPER_FAST);
msleep(1600);
//move towards botguy
create_stop();
msleep(500);
//section 3
set_servo_position(0, 1424);
msleep(1800);
set_servo_position(0, 1100);
msleep(1800);
set_servo_position(0, 924);
msleep(1800);
set_servo_position(0, 724);
msleep(1800);
set_servo_position(3, 1024);
msleep(1800);
set_servo_position(0, 924);
msleep(1800);
set_servo_position(0, 1100);
msleep(1800);
set_servo_position(0, 1200);
msleep(2000);
//retrieve botguy
//section 4
create_drive_direct(-FAST, FAST);
msleep(445);
create_stop();
create_drive_direct(-FAST, -FAST);
msleep(2000);
create_stop();
create_drive_direct(FAST, -FAST);
msleep(445);
create_stop();
create_drive_direct(-FAST, -FAST);
msleep(900);
create_stop();
msleep(1600);
create_stop();
set_servo_position(3, 0);
set_servo_position(0, 1800);
msleep(1200);
set_servo_position(0, 1200);
msleep(1000);
set_servo_position(0, 1800);
msleep(1000);
create_drive_direct(1000, 1000);
msleep(800);
create_drive_direct(-1000, -1000);
msleep(800);
DRIVE(SUPER_FAST);
MSLEEP(800);
TURN(-SUPER_FAST, SUPER_FAST);
MSLEEP(460);
DRIVE(SUPER_FAST);
MSLEEP(1380);
TURN(SUPER_FAST, -SUPER_FAST);
MSLEEP(425);
DRIVE(SUPER_FAST);
SLEEP(1.5);
while(get_create_lbump(0.01) == 0)
{
DRIVE(SUPER_FAST);
}
TURN(-SUPER_FAST, SUPER_FAST);
MSLEEP(435);
TURN(SUPER_FAST, 975);
MSLEEP(500);
while(get_create_rbump(0.01) == 0)
{
TURN(SUPER_FAST, 990);
}
create_stop();
create_disconnect();
}
int main()
{
struct cbcRobot robot = {0,3,150,55,1100}; //Define the robot. 0 and 3 are the motor ports,
//150 mm is the wheel distance
//55 is wheel diameter, 1100 is ticks per rotation
light_it_up(4); //Wait for the light to turn on
set_servo_position(arm_servo, top_pos); //Set the arm servo to the top position
shut_down_in(118.0); //Shut down in 118 seconds
//msleep(5000); //Wait 5 seconds
float initial_time=seconds(); //Create a variable that records the initial time
cbc_align(400); //Align with both of the top hats on the black line
ao();
cbc_align_white(300);
lower_arm(600); //Lower claw and open arm
open_claw(600);
cbc_straight(140, 800); //Straight out to get away from pvc
wait_for_cbc();
cbc_arc_left(160, 135, 100); //Get past pvc to lower arm and open claw
wait_for_cbc();
cbc_straight(180, 800); //Go straight into tribbles
wait_for_cbc();
close_claw(1000); //Close claw on tribbles
cbc_straight(280, 800);
wait_for_cbc();
raise_arm(1200); //Raise the arm
msleep(800);
cbc_arc_right(180, 34, 80); //Arc right to get into position to approach the lattice wall
wait_for_cbc();
cbc_touch(700, 5000); //Bump into lattice wall
msleep(300);
cbc_straight(-100, 600); //Back up 10 cm from wall
wait_for_cbc();
open_claw(800); //Open claw to release tribbles and turn 94 degrees right
cbc_spin(-94, 500);
wait_for_cbc();
cbc_straight(120, 700); //Go forward into the turnstile and lower arm to mid position
arm_mid(2000);
msleep(1200);
cbc_spin(20, 500); //Turn 20 degrees left to hit the turnstile out of the way
wait_for_cbc();
msleep(200);
cbc_spin(-16, 700); //Turn back 16 degrees to the right and close the claw
close_claw(800);
wait_for_cbc();
cbc_straight(-280, 800); //Go backwards to get into position to grab the tribbles
wait_for_cbc();
lower_arm(1000); //Lower the arm and open the claw
msleep(800);
open_claw(800);
msleep(1000);
cbc_spin(8,700); //Turn 8 degrees to get ready to grab the tribbles
wait_for_cbc();
cbc_arc_left(1500, 6, 100); //Go nearly straight into the tribbles
wait_for_cbc();
close_claw(200); //Go forward and grab the tribbles
cbc_straight(200, 600);
wait_for_cbc();
close_claw(600);
msleep(800);
cbc_straight(-60, 600); //Back up to clear the turnstile
raise_arm(1000);
wait_for_cbc();
cbc_spin(4,700); //Turn slightly left
wait_for_cbc();
follow_tape_left(600); //Run into the turnstile for horizonal alignment
cbc_straight(-100, 400); //Back up before crossing over to the other side
wait_for_cbc();
cbc_spin(90, 500); //Turn 90 degrees to the left
wait_for_cbc();
cbc_straight(-80, 600); //Back up and align with the black tape
wait_for_cbc();
cbc_align(400);
ao();
cbc_straight(-50, 300); //Back up and align with the black tape
wait_for_cbc();
cbc_align(400);
ao();
cbc_straight(540,800); //Go forward to the other side
wait_for_cbc();
open_claw(1000); //Open the claw to release tribbles and arc right approximately 100 degrees
mrp(robot.leftWheel,780,2340);
mrp(robot.rightWheel,400,1200);
bmd(robot.leftWheel);
bmd(robot.rightWheel);
msleep(200L);
lower_arm(1400);//Go forward and lower the arm
msleep(500L);
cbc_straight(370, 800);
wait_for_cbc();
close_claw(1000); //Go forward and close the claw
cbc_straight(300,800);
wait_for_cbc();
raise_arm(1500); //Raise the arm
msleep(800);
cbc_spin(-73,500); //Turn 73 degrees right
wait_for_cbc();
open_claw(1000); //Run into the lattice wall and open the claw to release the tribbles
cbc_touch(500, 5000);
cbc_straight(-80, 600); //Back up 8 cm from wall
wait_for_cbc();
cbc_spin(-92, 500); //Turn 92 degrees right and close the claw
close_claw(1000);
wait_for_cbc();
cbc_straight(-80, 600); //Go backwards to get into position to grab the tribbles
wait_for_cbc();
lower_arm(1500); //Lower the arm and open the claw
msleep(800);
open_claw(800);
msleep(1000);
cbc_spin(4,700); //Turn a bit in order to put the robot in an optimal tribble gathering position
wait_for_cbc();
cbc_arc_left(1500, 6, 100); //Go nearly straight into the tribbles
wait_for_cbc();
close_claw(200); //Go forward and grab tribbles
cbc_straight(180, 600);
wait_for_cbc();
close_claw(600);
msleep(800);
cbc_straight(-80, 400); //Back up to clear the turnstile
wait_for_cbc();
raise_arm(1200); //Raise the arm
msleep(500);
cbc_straight(220, 700); //Go forward into the turnstile
wait_for_cbc();
cbc_spin(80, 500); //Turn 80 degrees to the left
wait_for_cbc();
cbc_straight(-80, 600); //Back up and align with the black tape
wait_for_cbc();
cbc_align(400);
ao();
cbc_straight(-50, 300); //Back up and align with the black tape
wait_for_cbc();
cbc_align(400);
ao();
//cbc_straight(1080, 800); //Old go forward function
//wait_for_cbc();
mrp(robot.rightWheel,825,7700); //Go forward and correct the non-straightness of the motors
mrp(robot.leftWheel,800,7700);
wait_for_cbc();
cbc_touch(500, 5000); //Run into the wall
msleep(300);
cbc_straight(-125, 600); //Back up 12.5 cm from wall
wait_for_cbc();
cbc_spin(90, 500); //Turn 90 degrees to the left
wait_for_cbc();
while(analog10(et_right)<850) //While the tape is not seen by the right top hat
{
mav(robot.rightWheel,615); //Go forward
mav(robot.leftWheel,600);
msleep(25L);
}
off(robot.rightWheel); //Turn the motors off
off(robot.leftWheel);
cbc_straight(240, 700); //Go forward to get under the injection chute
wait_for_cbc();
msleep(8000); //Wait for the injection chute tribbles to be loaded up
cbc_straight(250, 700); //Go forward into the wall
wait_for_cbc();
cbc_straight(-40, 600); //Back up 4 cm from pvc
wait_for_cbc();
arm_mid(1000); //Lower the arm to the mid position and open the claw
msleep(800);
open_claw(800);
msleep(800);
cbc_straight(-40, 600); //Back up 4 cm from pvc
wait_for_cbc();
close_claw(1000); //Turn around 180 degrees and close the claw
cbc_spin(180,600);
wait_for_cbc();
cbc_straight(-80, 480); //Back up to wall
wait_for_cbc();
lower_arm(1400); //Lower the arm
msleep(800L);
mrp(dump_mot, 200, 500); //Dump the tribble into the MPA
bmd(dump_mot);
msleep(1000);
mrp(dump_mot, 200, -500); //Dump the tribble into the MPA
bmd(dump_mot);
mrp(dump_mot, 200, 500); //Dump the tribble into the MPA
bmd(dump_mot);
mrp(dump_mot, 200, -500); //Dump the tribble into the MPA
bmd(dump_mot);
float time = seconds() - initial_time; //Create a variable that represents the time taken for the robot to complete its program
cbc_display_clear(); //Clear the CBC display and then print the time the robot took to complete its program
printf("Time: %f",time);
return 0;
}
