void create_lineup(){//lines up the create on a black line
int done = 0;
float tstart = seconds();
int retry = 1;//don't retry ever
while(done < 4){//WORK ON THIS
msleep(5);
int lspd,rspd;
lspd = rspd = 0;
if (lcliff > 800) lspd = 20;
if (lcliff < 500) lspd = -20;
if (rcliff > 800) rspd = 20;
if (rcliff < 500) rspd = -20;
if (seconds()-tstart > 4){lspd/=2;rspd/=2;}
if (seconds()-tstart > 6){
create_stop();
return;//failure, timeout
}
//printf("\n%6d%6d",lcliff,rcliff);
create_drive_direct(lspd,rspd);
if (lspd == rspd && lspd == 0)
done++;
else
done=0;
}
create_stop();
}
void GotoBox()
{
while (get_create_lbump(.1) == 0 && get_create_rbump(.1) == 0)//drive backwards until hit pvc w/ bumper
{
create_drive_straight(200); //unless arm is strong enough to hold
}
create_stop();
ClearWall(); //move away from pvc to turn
if(Side == 0) //turn towards outer starting box -- now facing our outer starting box w/ blocks in claw
{
TurnCCW();
}
if(Side == 1)
{
TurnCW();
}
create_stop();
ForwardTouch(); //forward till front touch sensor ==1
create_drive_straight(132); //one cube length (5 inches) + 5 mm safety // drive backwards
sleep(1);
Release(); // realease claw w/ 2 blocks -- block now in starting box
create_drive_straight(-75);
sleep(1);
create_stop(); //make sure cubes are touching pvc
}
void backFromTouch(int stack_num)
{
switch (stack_num) {
case 0:
driveAtMm(GRAB_DIST*2);
sleep(.5);
run_for(.5, clawGrab);
create_stop();
clawGrab();
break;
case 1:
driveAtMm(STACK1_DIST*2); //touch to stack height 1
sleep(.2);
run_for(.8, clawGrab);
create_stop();
clawGrab();
break;
case 2:
driveAtMm(STACK2_DIST); //touch to stack height 2
sleep(.7); //run_for(.8, clawStack1);
create_stop();
clawStack1();
break;
case 3:
driveAtMmFor(STACK3_DIST,1.2); //touch to stack height 3
break;
default:
break;
}
}
void line_follow()
{
int ldrive = 100;
int hdrive = 300;
int mdrive = 200;
if (LFCLIFF >= T && RFCLIFF <= T)
{
create_drive_direct(hdrive , ldrive);
}
else if (LFCLIFF <= T && RFCLIFF >= T)
{
create_drive_direct(ldrive , hdrive);
}
else if (LFCLIFF >= T && RFCLIFF >= T)
{
create_drive_direct(hdrive , hdrive);
}
else if (LFCLIFF <= T && RFCLIFF <= T)
{
create_drive_direct(mdrive , mdrive);
}
else if (LCLIFF >= T)
{
create_stop();
turn_while(250 , 77 , 1);
}
else if (RCLIFF >= T)
{
create_stop();
turn_while(250 , 77 , -1);
}
}
int find_item()
{
camera_update();
point2 mcenter = get_object_center(1,0);
set_servo_position (0 ,400);
while(get_object_count(1) == 0){
camera_update();
create_spin_CCW(25);
}
create_stop();
sleep(1);
create_spin_CW(15);
msleep(100);
while(mcenter.x < 70 || mcenter.x > 100){
camera_update();
mcenter = get_object_center(1,0);
if(mcenter.x < 70){
create_spin_CCW(15);
camera_update();
mcenter = get_object_center(1,0);
}
if(mcenter.x < 100){
create_spin_CW(15);
camera_update();
mcenter = get_object_center(1,0);
}
if(mcenter.x > 70 && mcenter.x < 100){
create_stop();
}
}
beep();
}
//This function blocks and does a pretty accurate spin. Note that the function will
//not return until the spin is complete
//CAUTION, requesting the robot to spin more than about 3600 degrees may never terminate
//the 9/07 firmware update is required for this works. Returns -1 if error
int create_spin_block(int speed, int angle)
{
char buffer[7];
char *bptr = buffer;
int r[7]={18,7,9,24,12,15,21},v[7],i,b;
long lenc=0L, renc=0L,slenc,srenc,flenc,frenc;
float offset, ticsPerMM=7.8324283, rad=129.0, pi=3.1415926;
CREATE_BUSY;
create_write_byte(8);//version
CREATE_WAITFORBUFFER(buffer,7,-99)
for(i=0;i<7;i++){
v[i]=(*(bptr++));
}
CREATE_FREE;
if(!(v[1]==r[1] && v[2]==r[2] && v[3]==r[3])){
printf("create_spin_block\n requires Create Firmware dated\n 9/24/2007\n");
printf("Yours is:%d/%d/%d\nFunction aborted\n", v[2], v[3], v[1]+2000);
return(-1);
}
_create_get_raw_encoders(&slenc, &srenc);
offset=(float)angle*pi*rad*ticsPerMM/180.0;
if(angle>0){create_drive_direct(speed,-speed);}
else {offset=-offset; create_drive_direct(-speed,speed);}
flenc=slenc+(long)offset;
frenc=srenc+(long)offset;
if(frenc > 65535L && flenc > 65000L){
while(lenc >= slenc)_create_get_raw_encoders(&lenc, &renc);
while(lenc < flenc-65536L)_create_get_raw_encoders(&lenc, &renc);
create_stop();}
else{
while(lenc < flenc && renc < frenc)_create_get_raw_encoders(&lenc, &renc);
create_stop();
}
return(0);
}
int main()
{
wait_for_light(4);
printf("Hello, World!\n");
create_connect();
create_drive_direct(200 , 200);
msleep(2000);
create_spin_CCW ( 100 );
msleep(2000);
create_stop();
create_drive_direct(200 , 200 );
msleep(4000);
create_stop();
create_drive_direct (200 , 200);
msleep(5000);
create_stop();
create_spin_CCW ( 100 );
msleep(2000);
create_drive_direct (200 , 200);
msleep(3000);
create_spin_CCW ( 100 );
msleep(2000);
create_cover();
create_stop();
create_disconnect();
return 0;
}
int main()
{
create_connect();
init_servo();
//light_it_up(LIGHT_PORT);
lightstart(LIGHT_PORT,120.0);
create_full();
create_drive_segment(HIGH_SPEED, -150);
start_process(set_top);
create_drive_arc(HIGH_SPEED, -200, 155);
create_drive_segment(HIGH_SPEED, -365);
create_drive_arc(HIGH_SPEED, 90, -64);
create_drive_segment(HIGH_SPEED, -85);
create_cease();
set_top();
align_twall();
grab_top_de();
create_spin_angle(400,-165);
create_drive_segment(HIGH_SPEED, -320);
//align_wall();
create_stop();
create_sync();
dump_kelp_de();
create_drive_segment(HIGH_SPEED,30);//go away from starting box
create_spin_angle(HIGH_SPEED, -78);//turn to gate part 1
create_drive_segment(HIGH_SPEED, 938);//go to gate
create_drive_arc(HIGH_SPEED,185,84);//arc to gate
//create_drive_time(HIGH_SPEED,1800);//go through the gate
/*
create_drive_segment(HIGH_SPEED,1470);
create_drive_segment(HIGH_SPEED,-100);
create_spin_angle(HIGH_SPEED,78);
*/
create_cease();
create_drive_straight(HIGH_SPEED);//go onto oppponent's side of board
msleep(1400);
create_stop();
create_arc(350,550);//arc to the opponent's mpa
msleep(2500L);
create_stop();//pause to keep the creat
create_drive_straight(HIGH_SPEED);//go onto oppponent's side of board
msleep(750);
create_stop();
create_drive_straight(-HIGH_SPEED);//go onto oppponent's side of board
msleep(5000);
create_stop();
/*
create_drive_segment(500,-500);//go past the IC
create_drive_arc(320,-250,25);//arc to
create_drive_arc(320,250,-8);
*/
//create_drive_segment(HIGH_SPEED,-750);
//create_drive_arc(HIGH_SPEED,100,84);
//create_drive_segment(HIGH_SPEED,1300);
}
/*Function definitions go below.*/
void CenterCamera2() //CAMERA MUST BE AT CENTER OF CREATE, POINTING Right RESPECT TO CREATE
{
if (track_x(0,0) > 81) //if blocks are bd
{
for(create_spin_CCW(100); track_x(0,0) > 81; track_update()){} //WHAT TRACK # IS RED? MAKE SURE IT DOESNT DETECT BOTGUY -- remember to calibrate CBC to blocks
create_stop();
}
if (track_x(0,0) < 78) //if blocks are fd
{
for(create_spin_CW(100); track_x(0,0) < 78; track_update()){} //WHAT TRACK # IS RED? MAKE SURE IT DOESNT DETECT BOTGUY -- remember to calibrate CBC to blocks
create_stop();
}
}
void ClearWall()
{
if(get_create_lbump(.1) == 1 || get_create_rbump(.1) == 1)
{
create_drive_straight(-132); //one cube length (5 inches) + 5 mm safety
sleep(1);
create_stop();
}
if(digital(TouchSensor) == 1)
{
create_drive_straight(132);
sleep(1);
create_stop();
}
}
void *control(int *unused)
{
while(1)
{
if(mission>6)
{
printf("invalid value in mission: %d\n",mission);
}
else
{
switch(mission)
{
case FRONT:
drive_random();
break;
case CHARGE:
charge();
break;
case IDLE:
create_stop();
break;
default:
track(mission);
}
}
create_charge=create_get_charge();
}
}
/* This program will move Robot A accordig to its assignment. */
int main()
{
printf("Hello, World!\n");
create_connect();
int rport=1; // identify ports used, might need to change
int threshold=500; // light threshold in the room
int high=100, low=-10; //motor power for arc. Will need to change to perfect drive.
while (b_button()==0) {} // start after pushing B
/* To be replaced with wait_for_light(port#); so that light starts the robot. Right now it is started with B*/
while (a_button()==0){ // stop after pushing A
/* to be relplaced with a button sensor. Such as digital(Port #); so that the claw grabs the transport when the sensor(s) are pushed.*/
while (analog10(rport) > threshold) { // continue until not dark
create_drive_direct(-10,100); // arc left
if (a_button()!=0) break; } // or button pressed
// to be replaced with a sensor
while (analog10(rport) <= threshold){ // continue until dark
create_drive_direct(100,-10); // arc right
if (a_button()!=0) break; } // or button pressed
// to be replaced with a sensor
create_stop();
//all stop because button was pushed.
}
create_disconnect();
return 0;
}
void create_virtual_bump(int speed, int direction) {
create_drive(speed, direction);
while (create_get_sensor(CURRENT) > CURRENT_THRESHOLD) {
//msleep(20);
}
create_stop();
}
void drive_random()
{
srand ( time(NULL) );
int direction=rand()%361-180; //-180 to +180
if(direction<0)
{
set_create_total_angle(0);
create_drive_direct(-200,200);
while(get_create_total_angle(0.1)>direction);
}
else
{
set_create_total_angle(0);
create_drive_direct(200,-200);
while(get_create_total_angle(0.1)<direction);
}
int i=0;
while(i<1000)
{
if(get_create_lbump(0.1)||get_create_rbump(0.1)) //if bumper pressed
{
i=2000;
}
else
{
create_drive_direct(300,300);
}
i++;
msleep(1);
}
create_stop();
}
//generic move forward function calibrated in inches
void forward (int inches){
create_drive_straight(SPEED_FWD);
int time_conversion = inches * FORWARD_CONSTANT;
msleep(time_conversion);
create_stop();
}
//generic move backward function calibrated in inches
void backward (int inches){
create_drive_straight(SPEED_BWD);
int time_conversion = inches * BACKWARD_CONSTANT;
msleep(time_conversion);
create_stop();
}
int main()
{
printf("Line Follow."); // announce the program
sleep(1.0); // wait 1 second
create_connect(); // Open the connection between CBC and Create
int ana0 = 0, ana1 = 0; // variables to store the results of the analog sensors
create_full(); // We don't care about safety
while(get_create_lbump(0) == 0 && get_create_rbump(0) == 0){ //while the bumper is not pressed
ana0 = analog10(0); // left sensor
ana1 = analog10(1); // right sensor
printf("analog 0: %d\n", ana0);// print results
printf("analog 1: %d\n", ana1);
if((ana0 < 200) && (ana1 > 200)) // if the left sensor is off and right is on black line
{
create_spin_CW(128); // spin the create Clock Wise
}else if((ana1 < 200) && (ana0 > 200)) // else if reversed
{
create_spin_CCW(128); // spin Counter Clock Wise
}else if((ana0 < 200) && (ana1 < 200)) // else neither is on the line
{
create_drive_straight(180); // drive straight
}
}
create_stop(); // Stop the Create
create_disconnect(); // Disconnect the Create
}
void detectBlock() { //200
/*
printf("%d\n", ET_FRONT);
drive(-200);
while (ET_FRONT < ET_FRONT_BLOCK) {printf("%d\n", ET_FRONT); sleep(.1);}
driveAtMmFor(-200, .5);*/
printf("%d\n", ET_FRONT);
drive(-200);
//clawOpen();
/*
while (ET_FRONT < ET_FRONT_BLOCK) {
while (get_servo_position(SERVO_ARM_LEFT_PORT) < SERVO_ARM_LEFT_STACK1)
{
set_servo_position(SERVO_ARM_LEFT_PORT, SERVO_ARM_LEFT+30);
set_servo_position(SERVO_ARM_RIGHT_PORT, SERVO_ARM_RIGHT-30);
sleep(.01);
}
while (get_servo_position(SERVO_ARM_LEFT_PORT) > SERVO_ARM_LEFT_STACK1)
{
set_servo_position(SERVO_ARM_LEFT_PORT, SERVO_ARM_LEFT-30);
set_servo_position(SERVO_ARM_RIGHT_PORT, SERVO_ARM_RIGHT+30);
sleep(.01);
}
armPosBlockStack1();*/
while (ET_FRONT < ET_FRONT_BLOCK) {
printf("A");
printf("%d\n", ET_FRONT);
sleep(.1);
}
//driveAtMmFor(-200, .5);
create_stop();
}
void detectBlock(){ //200
printf("%d\n", ET_FRONT);
drive(-200);
while (ET_FRONT < ET_FRONT_BLOCK) {printf("%d\n", ET_FRONT); sleep(.1);}
driveAtMmFor(-200, .5);
create_stop();
}
// returns the serial connections on XBC to normal communications over the USB port.
// Turns of play LED and returns power light to green
void create_disconnect() {
create_play_led(0);
create_power_led(0,255);
create_stop();
g_create_connected=0;
serial_quit();
}
void move_until_line() {
create_drive(100, FORWARDS);
while (get_create_rfcliff_amt() > CREATE_THRESHOLD) {
display_printf(0, 0, "%4i", get_create_rfcliff_amt());
}
create_stop();
}
/*
* Class: cbccore_low_Create
* Method: create_stop
* Signature: ()V
*/
JNIEXPORT void JNICALL Java_cbccore_low_Create_create_1stop(JNIEnv *env, jobject obj)
{
#ifdef CBC
create_stop();
#else
printf("Java_cbccore_low_Create_create_1stop stub\n");
#endif
}
void clearWall()
{
if(TOUCH_BACK_AND) driveAtMmFor(-50,1);
else if(TOUCH_FRONT) driveAtMmFor(50,1);
else if(TOUCH_BACK_OR){
if(TOUCH_BACK_LEFT){
create_spin_CW(50);
while(!TOUCH_BACK_RIGHT){}
create_stop();
}
if(TOUCH_BACK_RIGHT){
create_spin_CCW(50);
while(!TOUCH_BACK_LEFT){}
create_stop();
}
}
}
void TurnCW()
{
while(get_create_total_angle(0.1) > -90)
{
create_spin_CW(500);
}
create_stop();
}
void ForwardTouch ()
{
while (analog(TouchSensor) == 0)
{
create_drive_straight(-500);
}
create_stop();
}
void TurnCCW()
{
while(get_create_total_angle(0.1) > 90)
{
create_spin_CCW(500);
}
create_stop(); //TEST THESE. IS IT TOO FAST FOR LAG TIME?
//IS IT BETTER TO GO MANUAL? (219.44 for 1 second)
}
void CenterCamera() //CAMERA MUST BE AT CENTER OF CREATE, POINTING RIGHT RESPECT TO CREATE
{
create_drive_straight(-200);
while (track_x (0,0) > 84) //WHAT TRACK # IS RED? MAKE SURE IT DOESNT DETECT BOTGUY
{
track_update();
}
create_stop();
if (track_x (0,0) < 75)
{
create_drive_straight(200);
while (track_x (0,0) < 75)
{
track_update();
}
create_stop();
}
}
int main()
{
create_connect();
float x=.45;
if(black_button() == 1)
{
create_drive_straight(-100);
sleep(10);
create_stop();
}
while(left_button() == 0)
{
while(black_button() == 0)
{
if (a_button() == 1)
{
x=x+.001;
printf("%f\n", x);
sleep(.5);
}
if (b_button() == 1)
{
x=x-.001;
printf("%f\n", x);
sleep(.5);
}
if (up_button() == 1)
{
x=x+.0001;
printf("%f\n", x);
sleep(.5);
}
if (down_button() == 1)
{
x=x-.0001;
printf("%f\n", x);
sleep(.5);
}
}
create_spin_CW(450);
sleep(x);
create_stop();
}
}
void detectPVC(){ //400
printf("%d\n", ET_FRONT);
drive(-400);
while (ET_FRONT < ET_FRONT_PVC-100) {printf("%d\n", ET_FRONT); sleep(.1);}
drive(-200);
printf("%d\n", ET_FRONT);
while (ET_FRONT < ET_FRONT_PVC) {printf("%d\n", ET_FRONT); sleep(.1);}
create_stop();
driveAtMmFor(-100,1);
}
void detectBlockSlow() {
printf("%d\n", ET_FRONT);
drive(-100);
while (ET_FRONT < ET_FRONT_BLOCK) {
printf("A");
printf("%d\n", ET_FRONT);
sleep(.1);
}
driveAtMmFor(-100, 1);
create_stop();
}
