void calibrate(){
printf("Bitte setze alle Sensoren auf den schwarzen Streifen \n");
set_b_button_text("calibrate");
while(!b_button()){
printf("left "); printf("%i",analog10(LEFT_SENSOR)); printf("\n");
printf("right "); printf("%i",analog10(RIGHT_SENSOR)); printf("\n");
msleep(100);
display_clear();
}
right_blk = analog10(RIGHT_SENSOR);
left_blk = analog10(LEFT_SENSOR);
mid_blk =analog10(MIDDLE_SENSOR);
set_b_button_text("start");
msleep(500);
printf("left "); printf("%i",left_blk); printf("\n");
printf("right "); printf("%i",right_blk); printf("\n");
printf("middle_black"); printf("%i",mid_blk); printf("\n");
while(!b_button()){}
set_a_button_text("Press-");
set_b_button_text("-Red-");
set_c_button_text("-Stop");
}
int main()
{
printf("press a to send hydro\npress b to send solar\npress up to send flag\n\n");
while(!a_button() && !b_button() && !up_button());
if(a_button())
{
while(a_button());
init_captain_planet_with_our_powers_combined();
kill_process(TIMER_WATCHDOG_ID);
printf("result of charge: %d\n",charge(HYDRO));
}
else if(b_button())
{
while(b_button());
init_captain_planet_with_our_powers_combined();
kill_process(TIMER_WATCHDOG_ID);
printf("result of charge: %d\n",charge(SOLAR));
}
else if(up_button())
{
while(up_button());
init_captain_planet_with_our_powers_combined();
kill_process(TIMER_WATCHDOG_ID);
printf("result of charge: %d\n",charge(FLAG));
}
kill_process(TIMER_PROCESS_ID);
serializer_disconnect();
}
int main()
{
drone_connect();
printf("Press A for SEEDING, B for PUSH_BOTGUY\n");
while (1) {
if (a_button())
{
while (a_button());
msleep(500);
printf("Running SEEDING program.\n");
start(SEEDING_SERVO_POSITION);
seeding();
}
else if (b_button())
{
while (b_button());
msleep(500);
printf("Running PUSH_BOTGUY program.\n");
start(PUSH_BOTGUY_SERVO_POSITION);
push_botguy();
}
}
// Don't need the rest anymore.
// start(SEEDING_SERVO_POSITION);
// start(PUSH_BOTGUY_SERVO_POSITION);
// seeding();
// push_botguy();
return 0;
}
void whichSide()
{
printf ("Select orientation of the robot compared to botguy.\n'A' button for left of bot guy.\n'B' button for right of bot guy.\n");
while (b_button() == 0 && a_button() == 0)
{
}
if (b_button() == 1)
{
Side = right;
cbc_display_clear();
printf ("Robot is on right of botguy.\n");
sleep(1);
}
if (a_button() == 1)
{
Side = left;
cbc_display_clear();
printf ("Robot is on left of botguy.\n");
sleep(1);
}
if (Side == right)
{
IR = 4;
}
if (Side == left)
{
IR = 6;
}
}
void testArc(){
float time1, time2;
cbc_display_clear();
set_create_total_angle(0);
printf("//Press Black Button to start/stop arc\n");
while(black_button()) while(!black_button()){}
time1 = seconds();
create_drive(-100,-580);
while(!black_button()); time2 = seconds(); stop();
printf("%fsecs\t %dmm\n",time2-time1,get_create_distance(0));
while(!black_button());
clawSB();
armPosSB();
printf("\npress A for too far, B for not far enough, Black for just right\n");
while(!done){
if(a_button()){
arc_value = arc_value-5;
done = 1;
}
if(b_button()){
arc_value = arc_value+5;
done = 1;
}
if(black_button()) done = 1;
}
printf("arc = %d",arc_value);
printf("press black button to exit");
while(!black_button());
}
int main()
{
X=0;
create_connect();
set_create_total_angle(0);
printf("X\tAngle");
while(black_button()==0)
{
if (a_button()==1)
{
turnHalfCCW();
printf("%d\t%d\n",X,get_create_total_angle(0.1));
}
if (b_button()==1)
{
turnHalfCW();
printf("%d\t%d\n",X,get_create_total_angle(0.1));
}
if (left_button()==1)
{
TurnCCW();
printf("%d\t%d\n",X,get_create_total_angle(0.1));
}
if (right_button() == 1)
{
TurnCW();
printf("%d\t%d\n",X,get_create_total_angle(0.1));
}
}
}
/* 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 strategySelect()
{
printf("Press A for luggage carts\n"); //Strategy 1
printf("Press B for tower tip\n"); //Strategy 2
printf("Press <- for biofuel block\n"); //Strategy 3
printf("Press -> for center defense\n"); //Strategy 4
while (black_button() == 0)
{
if (a_button() == 1)
{
printf("You picked luggage carts\n");
printf("Press black button to confirm\n");
strategy = 1;
}
if (b_button() == 1)
{
printf("You picked tower tip\n");
printf("Press black button to confirm\n");
strategy = 2;
}
if (left_button() == 1)
{
printf("You picked biofuel block\n");
printf("Press black button to confirm\n");
strategy = 3;
}
if (right_button() == 1)
{
printf("You picked center defense\n");
printf("Press black button to confirm\n");
strategy = 4;
}
}
}
/*Function definitions go below.*/
void Wait()
{
while(b_button() != 1)
{}
sleep(1);
beep();
}
/******** move the servo from 700 to 2048
*****************************************************************/
int main() {
// preset servo 1 position
set_servo_position(1,700);
printf("Press B to stop");
enable_servos(); // turn on servos
msleep(2000); // pause while it moves and user reads screen for 2 seconds
while((b_button()==0)) // while the button is not pressed
{ // move servo 1 in steps of 1348
set_servo_position(1,get_servo_position(1)+ 1348);
printf("servo at %d\n", get_servo_position(1));
msleep(200); // pause before next move
while((!a_button()) && (!b_button())) {}
}
disable_servos();
printf("Robot is done\n");
return 0;
}
void WhichSide () // For Left/Right of Botguy
{
printf ("Press and hold button for 1 second.\nA button for Left side, B button for Right side.\n");
while (b_button() == 0 && a_button() == 0)
{
}
if (b_button())
{
Side = Right;
printf ("On Right of Botguy. Set up lights.\n");
sleep(1);
}
if (a_button())
{
Side = Left;
printf ("On Left of Botguy. Set up lights.\n");
sleep(1);
}
}
int WhichSide (int Side)//need to specify whether 0 and 1 are right or left, respectively
{
printf ("press and hold button for 1 second");
while (a_button() == 0 && b_button() == 0)
{
sleep(1);
printf ("try again");
}
if (a_button() == 1)
{
Side = 0;//create is on the right side
printf ("set up lights");
}
if (b_button() == 1)
{
Side = 1;//create is on the left side
printf ("set up lights");
}
}
// General-purpose function for testing movement.
void test_robot(int speed, int distance, int direction)
{
clock_t start, end;
float seconds;
printf("Requested speed, distance = %3d %3d.\n",
speed, distance);
printf("Press the B button to continue.\n");
while (b_button() == 0);
while (b_button() == 1);
sleep(1.0);
start = clock();
if (direction == FORWARD)
{
forward(speed, distance);
}
else if (direction == BACKWARD)
{
backward(speed, distance);
}
else if (direction == LEFT)
{
left(speed, distance);
}
else if (direction == RIGHT)
{
right(speed, distance);
}
end = clock();
seconds = (end - start) / (float) CLOCKS_PER_SEC;
//seconds = seconds * 15.0 / 11.0; // CBC CLOCKS_PER_TICK is wrong, it seems
printf("This motion took %0.2f seconds.\n", seconds);
}
void Cali () {
// change black_sen_thresh
printf("Light values: (off tape)\n");
printf("Press any button to start collecting light data...\n");
while(a_button() == 0 || b_button() == 0 || c_button() == 0) {
msleep(100);
}
printf("Collecting light data..........\n");
int num = 10;
int i, light_sum = 0;
for(i = 0; i < num; i++) {
light_sum += analog10(lightR);
msleep(100);
}
int light_avg = light_sum / num;
printf("Light avg : %d\n", light_avg);
printf("\nDark values: (on tape)\n");
printf("Press any button to start collecting dark data...\n");
while(a_button() == 0 || b_button() == 0 || c_button() == 0) {
msleep(100);
}
int dark_sum = 0;
for(i = 0; i < num; i++) {
dark_sum += analog10(lightR);
msleep(100);
}
int dark_avg = dark_sum / num;
printf("Dark avg : %d\n", dark_avg);
printf("Calculating sensor threshold");
msleep(1000);
int sen_thresh = (light_avg + dark_avg) / 2;
printf("Your sensor threshold is %d", sen_thresh);
black_sen_thresh = sen_thresh;
}
int main()
{
int Side;
printf ("press and hold button for 1 second");
beep();
while (a_button() == 0 && b_button() == 0)
{
sleep(1);
printf ("try again");
}
if (a_button() == 1)
{
Side = 0;//create is on the right side
printf("right %d/n", Side);
printf ("set up lights/n");
}
if (b_button() == 1)
{
Side = 1;//create is on the left side
printf("left %d/n", Side);
printf ("set up lights/n");
}
}
void main()
{
calibrate();
enable_servos();
set_digital_pullup(BUTTON,1);
//set_servo_position(SERVO,800);
int on=1;
int intervall = 5;
while(!b_button()){
if(digital(BUTTON)){
stop();
msleep(400);
printf("button pressed");
on=(on+1)%2;
}
if(on){
drive_speed=analog10(SPEED_SENSOR)/10;
if(drive_speed>100){
drive_speed=100;
}
forward();
while(analog10(LEFT_SENSOR)>=left_blk-150){
turn_left() ;
set_servo_position(SERVO,servo_pos);
if(digital(BUTTON)){ on=(on+1)%2;msleep(100);}
if(servo_pos>=400||servo_pos<=0){intervall*=-1; }
servo_pos=servo_pos+intervall;
}
while(analog10(RIGHT_SENSOR)>=right_blk-150){
turn_right();
set_servo_position(SERVO,servo_pos);
if(digital(BUTTON)){ on=(on+1)%2;msleep(100);}
if(servo_pos>=400||servo_pos<=0){intervall*=-1; }
servo_pos=servo_pos+intervall;
}
set_servo_position(SERVO,servo_pos);
if(servo_pos>=400||servo_pos<=0){intervall*=-1; }
servo_pos=servo_pos+intervall;
}
else{
stop();
}
}
stop();
}
void calibrate(){
printf("Please press the claibrate Button");
set_b_button_text("calibrate");
while(!b_button()){}
display_clear();
printf("calibrating");
//claws down
motor(Motor_Up,Motor_down_speed);
while(!digital(Sensor_Down)){}
freeze(Motor_Up);
//wait so it doesn't f**k up
msleep(500);
//up for straffes seil
motor(Motor_Up,Motor_up_speed);
while(digital(Sensor_Down)){}
freeze(Motor_Up);
claw_close();
}
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();
}
}
int main()
{
X=0;
create_connect();
set_create_total_angle(0);
printf("X\tAngle");
cbc_display_clear();
printf("A to turn CCW 90deg.\tB to turn CW 90deg.\n");
printf("Left to turn CCW 45deg.\tRight to turn CW 45deg.\n");
printf("Down to force angle readjustment.\n");
while(black_button()==0)
{
if (a_button()==1)
{
turn(tspeed,45);
//turnHalfCCW();
printf("%d\t%d\n",X,get_create_total_angle(0.1));
}
if (b_button()==1)
{
turn(tspeed,-45);
//turnHalfCW();
printf("%d\t%d\n",X,get_create_total_angle(0.1));
}
if (left_button()==1)
{
turn(tspeed,90);
//turnCCW();
printf("%d\t%d\n",X,get_create_total_angle(0.1));
}
if (right_button() == 1)
{
turn(tspeed,-90);
//turnCW();
printf("%d\t%d\n",X,get_create_total_angle(0.1));
}
if (down_button() == 1)
{
turnAdjust();
printf("%d\t%d\n",X,get_create_total_angle(0.1));
}
}
}
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();
}
int main () {
printf("a button will run block mid\nb button will block the far entrance\n");
while(a_button() == 0 && b_button() == 0) {
}
if(a_button() == 1) {
printf("a button pressed, wallaby will run block mid\n");
StartUp();
blockMiddle();
}
else {
printf("b button pressed, wallaby will run block the far entrance\n");
StartUp();
blockFarEntrance();
}
// blockFarEntrance(); 6/6
ao();
return 0;
}
void main()
{
calibrate();
set_servo_position(CAM_SERVO,CAM_SERVO_POSITION_RIGHT);
camera_open(LOW_RES);
enable_servos();
set_analog_pullup(3,0);
while(!b_button()){
claw_at_linefollowing();
do {
camera_update();
linefolowing();
} while(get_object_count(green) ==0); //linefolowing bis er was sieht
if(get_object_bbox(0,0).width>BLOB_SIZE_TAKE&&get_object_bbox(0,0).height>BLOB_SIZE_TAKE){
takepom();
}
printf("kompletdurch \n");
}
disable_servos();
stop();
}
void main(){
int X = 0;
create_connect();
set_create_distance(0);
set_create_total_angle(0);
enable_servos();
armPosBlockStack1();
clawStack1();
/* create_drive(-500,-380);
sleep(2.3);
create_stop();
}
*/
// writeToFile();
printf("%f",seconds());
while(1) //black_button
{
if (a_button()){
testDrive();
}
if (b_button()){
testDetectPVC();
}
if (up_button()){
}
if (down_button()){
testTurnAdjust();
}
if (left_button()){
testTurnCCW();
}
if (right_button()){
testTurnCW();
}
}
}
int main()
{
set_a_button_text("pipe jumping");
set_b_button_text("center drive");
set_c_button_text("testing");
int strategy;
WAIT(a_button()||b_button()||c_button());
if(a_button())
{
strategy=PIPE_JUMP;
}
else if(b_button())
{
strategy=CENTER_DRIVE;
}
else//c
{
WAIT(side_button());
msleep(1000);
set_servo_position(TRIBBLE_ARM, TA_DOWN);
set_servo_position(TRIBBLE_CLAW, TC_OPEN);
set_servo_position(BLOCK_CLAW, BC_START);
set_servo_position(BASKET_ARM, BA_DOWN);
enable_servos();
msleep(2000);
forward(20, 60);
return 0;
//set_up_drive();//gets the servos and stuff in driving position
//test_driving();
reset_buttons();
set_up_drive();
printf("press black button to begin\n");
WAIT(side_button());
printf("starting in 2 seconds...\n");
msleep(2000);
back_line_follow_time(60, 5000);
return 9001;
}
WAIT(!a_button()&&!b_button());
set_a_button_text("light start");//only gets here if running one of the real code options
set_b_button_text("no ls");
set_c_button_text("-");
boolean l_s=false;//whether or not is doing light start
WAIT(a_button()||b_button());
if(a_button())//if b, does nothing-->l_s already false
l_s=true;
reset_buttons();
if(strategy==PIPE_JUMP)
set_up_jump();
else//drive
set_up_drive();
if(l_s)
{
light_start(LIGHT_SENSOR);
}
else//no light start
{
printf("press black button when ready\n");
WAIT(side_button());
printf("starting in 2 seconds...\n");
msleep(2000);
}
shutdownin(239.5);
start();//timing
if(strategy==PIPE_JUMP)
{
ready_to_jump();
time_drive(-90, -90, 1300);//jump!
servo_set(TRIBBLE_ARM, TA_UP,.3);//move the claw up so it can square up
time_drive(-50, -50, 1500);//get towards the pipe
physical_squareup(false);//square up on the back
time_drive(50, 50, 2500);//go back towards the other wall
physical_squareup(true);//square up on the front
time_drive(-50, -50, 1000);//get back to the center
right(87, 0, 50);//turn towards the edge
time_drive(-60, -60, 1500);//move towards edge
physical_squareup(false);//square up on the outside of the field
tribble_claw_drop();//drop the claw-->plow position
time_drive(68, 60, 1100);//arc into the wall to wall follow to the blocks
//forward(5.25, 60);//move to block position
grab_blocks();//grab the blocks...
forward(20, 60);//plow the tribbles!
servo_set(TRIBBLE_CLAW, TC_CLOSE, .7);//close claw to catch tribbles
servo_set(TRIBBLE_ARM, TA_UP, 1);//put the arm up to get across the center
forward(20, 60);//get across the center
servo_set(TRIBBLE_ARM, TA_START, .5);//push into the ground so the claw doesn't jump
servo_set(TRIBBLE_CLAW, TC_OPEN, .8);//back into plow position
servo_set(TRIBBLE_ARM, TA_DOWN, .1);//back to drive position
msleep(250);
forward(7, 60);//get to dumping location
nowstr("first dump started at");
drive_dump();//dump...
forward(7, 60);//replow the tribbles (maybe get a few extra)
back(2, 60);//get to optimal grab location
tribble_claw_dump();//put the tribbles in the basket
back(5, 60);//back to dump location
tribble_claw_drop();//put the claw back down
drive_dump();//dump again
nowstr("first dump finished at");
//forward(15, 60);//plow the second set of tribbles, get to block location
time_drive(66, 60, 3000);//arc towards the right wall to make sure it follows the right wall
grab_blocks();//grab the blocks...
forward(8, 60);//plow the remaining tribbles
servo_set(TRIBBLE_CLAW, TC_CLOSE, .4);//grab the tribbles
servo_set(TRIBBLE_ARM, TA_DUMP, 1);//raise the arm (to allow it to square up)
time_drive(60, 60, 1500);//get towards the wall
physical_squareup(true);//and square up on it
msleep(200);
back_line_follow(35.8, 60);//back up to the dumping location
servo_set(TRIBBLE_ARM, TA_DOWN, .5);//put the arm down to get it out of the way
nowstr("second dump started at");
drive_dump();//dump
tribble_claw_dump();//put the tribbles in the basket
tribble_claw_drop();//
drive_dump();//dump again
nowstr("second dump finished at");
servo_set(TRIBBLE_CLAW, TC_PART_OPEN, .3);//get the claw up and out of the way
servo_set(TRIBBLE_ARM, TA_UP, .3);
back(6, 60);//back up to get the caught tribbles
servo_set(TRIBBLE_ARM, TA_START, .3);//put the claw back down
servo_set(TRIBBLE_CLAW, TC_OPEN, .5);//
servo_set(TRIBBLE_ARM, TA_DOWN, .1);//
forward(38, 60);//push the tribbles (get any that went way down the field)
back(2, 60);//tribbles near edge of claw
tribble_claw_dump();
servo_set(TRIBBLE_CLAW, TC_CLOSE, .3);//close the claw, just for good measure
back_line_follow(30, 60);//get back to the dumping location
servo_set(TRIBBLE_ARM, TA_DOWN, .5);//get the arm out of the way
nowstr("third dump started at");
drive_dump();
nowstr("third dump finished at");
back_line_follow(17, 60);//get back across the center
right(180, 0, 50);//turn around
tribble_claw_drop();//put the claw down...
forward(25, 60);//and plow!
back(2, 60);//optimal grab location
tribble_claw_dump();//well duh...
servo_set(TRIBBLE_CLAW, TC_CLOSE, .3);//close the claw for good measure
time_drive(60, 60, 1500);//move towards the wall
physical_squareup(true);//and square up on it
back_line_follow(35.8, 60);//back up to the dumping location
servo_set(TRIBBLE_ARM, TA_DOWN, .5);//get the claw out of the way
nowstr("fourth dump started at");
drive_dump();//dump stuff
move_block_arm(BLA_MID);//get the block arm back out of the way
servo_set(BASKET_ARM, BA_UP, 1);//put the basket back up
time_drive(-50, -50, 1500);//one last chance to dump (stay there just cause...you never know)
}
else//drive strategy
{
forward(38, 90);//get towards the center
right(90, 0, 70);//turn towards the middle of the gap
forward(17, 90);//get through the gap and to the black tape
left(85, 0, 70);//turn parallel to the black tape
servo_set(TRIBBLE_ARM, TA_JUMP, .4);//get the claw into plow position
servo_set(TRIBBLE_CLAW, TC_PART_OPEN, .4);//
servo_set(TRIBBLE_ARM, TA_DOWN, .4);//
servo_set(TRIBBLE_CLAW, TC_OPEN, .4);//
}
nowstr("finished at");
return 42;
}
int main(void)
{
//Decs
int right_sensor= 0;
int center_sensor = 1;
int left_sensor = 2;
int dark = 500;
int light_dark = 500;
int last_turn = 0;
printf("Starting line following program\n");
printf("Center the robot and calibrate the left then right sensor to begin\n\n");
//While the B button is not pressed, wait for input
while(analog10(left_sensor) < 600)
{
}
printf("Left sensor is ready\n\n");
while(analog10(right_sensor) < 600)
{
}
printf("Right sensor is ready\n\n");
printf("Press B to begin\n");
while(b_button() == 0)
{
}
printf("Running\n\n");
move(25, 100);
while(1==1)
{
//While there is not turn available, keep on keeping on.
while(isTurnAvailable() == 0)
{
moveForward(last_turn);
}
printf("Turn is available\n");
if(isTurnAvailable() == 2)
{
leftTurn();
last_turn = 1;
}
if(isTurnAvailable() == 1)
{
rightTurn();
last_turn = 0;
}
}//End of while loop
printf("Done");
return 0;
}
int main()
{
msleep(2500);
set_analog_pullup(ET_s , 0);
extra_buttons_show(1); // show three extra buttons
set_a_button_text("COORDS"); // set the text of various buttons
set_b_button_text("POM SIZE");
set_c_button_text("BOTGUY SIZE");
set_x_button_text("CUBE SIZE");
lego.left.port = 0; // set motor ports
lego.right.port = 2;
camera_open(LOW_RES);
camera_update();
while (a_button() == 0) // press the a button to set the coordinates
{
camera_update();
target.green.x = get_object_center(0 , 0).x; // sets target coordinates (x)
target.green.y = get_object_center(0 , 0).y; // sets target coordinates (y)
printf("(%d , %d)\n" , target.green.x , target.green.y);
msleep(10);
}
while (b_button() == 0)
{
camera_update();
target.green.size = get_object_area(0 , 0);
target.orange.size = get_object_area(1 , 0);
printf("Orange Size = %d" , target.orange.size);
printf(" Green Size = %d\n" , target.green.size);
}
enable_servo(arm_servo);
enable_servo(push_servo);
enable_servo(basket_servo);
set_servo_position(arm_servo , ARM_UP);
set_servo_position(push_servo , P_DOWN);
set_servo_position(basket_servo , B_UP);
printf("(%d , %d)\n" , target.green.x , target.green.y);
while(a_button() == 0)
{
printf("%d , %d , %d\n" , get_left() , get_middle() , get_right());
}
while (1) // line follow until poms are seen
{
blob_update();
t_line_follow();
if (current.orange.size > target.orange.size && current.green.size > target.green.size)
break;
}
blob_update();
get_pom(); // pick up a pom
pom_push(); // push it into the basket
while (1) // turn to next pom
{
blob_update();
mav(lego.left.port , 300);
mav(lego.right.port , -300);
msleep(10);
if (current.green.size > target.green.size)
break;
}
blob_update();
get_pom(); // pick up pom
pom_push(); // push it into the basket
avoid_cubeguy(); // avoid the cube or botguy
avoid_booster();
while (1)
{
blob_update();
t_line_follow();
if (current.orange.size > target.orange.size && current.green.size > target.green.size)
break;
}
blob_update();
pom_push();
while (1)
{
blob_update();
mav(lego.left.port , 300);
mav(lego.right.port , -300);
msleep(10);
if (current.green.size > target.green.size)
break;
}
blob_update();
get_pom();
pom_push();
avoid_cubeguy();
int start_time = seconds();
int t;
while (1)
{
t_line_follow();
if (seconds() < start_time + t)
break;
}
while (1)
{
}
}
void kissSimComputerDraw()
{
int oriX=ksWindow.simWidth, oriY=0, i;
static char digitals[]="0 0 0 0 0 0 0 0";
static char buttons[]="0 0 0 0 0 0 0";
static char analogs[11][10]={" "," "," "," "," "," "," "," "," "," "," "};
static char actuators[4][25]={" "," "," "," "};
static char servos[4][25]={" "," "," "," "};
if(!glfwGetWindowParam(GLFW_OPENED)) return;
graphics_rectangle_fill(oriX,oriY,oriX+1,ksWindow.height,BLACK); // draw left border
draw_bg(oriX+5,oriY+9,0.36); // Botguy
NOBOLD g_printString(" CBC SIMULATOR",oriX+25, oriY+10,TEAL,1.0);
NOBOLD g_printString(" DIGITALS ",oriX+25, oriY+40,TEAL,1.0);
NOBOLD g_printString("0 1 2 3 4 5 6 7",oriX+25, oriY+55,TEAL,1.0);
NOBOLD g_printString(" BUTTONS ",oriX+25, oriY+90,TEAL,1.0);
//NOBOLD g_printString("< ^ V > A B .",oriX+30, oriY+105,TEAL,1.0);
NOBOLD g_printString("\xb \xd \xe \xc A B .",oriX+30, oriY+105,TEAL,1.0);
// hex b, c, d, e are left, right, up, and down arrows
NOBOLD g_printString(digitals,oriX+25, oriY+70,WHITE,1.0); // erase old digital values
NOBOLD g_printString(" ANALOGS ",oriX+25, oriY+140,TEAL,1.0);
for(i=0;i<8;i++){
digitals[2*i]=48+digital(i);
}
NOBOLD g_printString(digitals,oriX+25, oriY+70,BLACK,1.0); // write new digital values
NOBOLD g_printString(buttons,oriX+30, oriY+120,WHITE,1.0); // erase old button values
buttons[0]=48+!!left_button();
buttons[2]=48+!!up_button();
buttons[4]=48+!!down_button();
buttons[6]=48+!!right_button();
buttons[8]=48+!!a_button();
buttons[10]=48+!!b_button();
_bob.button=!!kiss_get_key('.');
buttons[12]=48+black_button();
NOBOLD g_printString(buttons,oriX+30, oriY+120,BLACK,1.0); // erase old button values
for(i=8;i<16;i++){//print out analogs
analogs[i-8][1]=(i<10)?48+i:48+i-10;
analogs[i-8][0]=(i<10)?32:49;
analogs[i-8][2]=':';
analogs[i-8][3]=' ';
NOBOLD g_printString(analogs[i-8],oriX+5, oriY+155+(15*(i-8)),WHITE,1.0);
convertNumToString(analog10(i),4,*analogs,10,i-8,3);
graphics_rectangle_fill(oriX+30,oriY+155+(15*(i-8)),oriX+157, oriY+165+(15*(i-8)),GRAY);
graphics_line(oriX+30+analog(i)/2, oriY+155+(15*(i-8)),oriX+30+analog(i)/2,oriY+165+(15*(i-8)),RED);
NOBOLD g_printString(analogs[i-8],oriX+5, oriY+155+(15*(i-8)),BLACK,1.0);
}
analogs[8][1]='X';
analogs[8][0]='A';
analogs[8][2]=':';
analogs[8][3]=' ';
NOBOLD g_printString(analogs[8],oriX+5, oriY+155+(15*(8)),WHITE,1.0);
convertNumToString(accel_x(),5,*analogs,10,8,3);
graphics_rectangle_fill(oriX+30,oriY+155+(15*(8)),oriX+157, oriY+165+(15*(8)),GRAY);
graphics_line(oriX+30+(accel_x()+2048)/32, oriY+155+(15*(8)),oriX+30+(accel_x()+2048)/32,oriY+165+(15*(8)),RED);
NOBOLD g_printString(analogs[8],oriX+5, oriY+155+(15*(8)),BLACK,1.0);
analogs[9][1]='Y';
analogs[9][0]='A';
analogs[9][2]=':';
analogs[9][3]=' ';
NOBOLD g_printString(analogs[9],oriX+5, oriY+155+(15*(9)),WHITE,1.0);
convertNumToString(accel_y(),5,*analogs,10,9,3);
graphics_rectangle_fill(oriX+30,oriY+155+(15*(9)),oriX+157, oriY+165+(15*(9)),GRAY);
graphics_line(oriX+30+(accel_y()+2048)/32, oriY+155+(15*(9)),oriX+30+(accel_y()+2048)/32,oriY+165+(15*(9)),RED);
NOBOLD g_printString(analogs[9],oriX+5, oriY+155+(15*(9)),BLACK,1.0);
analogs[10][1]='Z';
analogs[10][0]='A';
analogs[10][2]=':';
analogs[10][3]=' ';
NOBOLD g_printString(analogs[10],oriX+5, oriY+155+(15*(10)),WHITE,1.0);
convertNumToString(accel_z(),5,*analogs,10,10,3);
graphics_rectangle_fill(oriX+30,oriY+155+(15*(10)),oriX+157, oriY+165+(15*(10)),GRAY);
graphics_line(oriX+30+(accel_z()+2048)/32, oriY+155+(15*(10)),oriX+30+(accel_z()+2048)/32,oriY+165+(15*(10)),RED);
NOBOLD g_printString(analogs[10],oriX+5, oriY+155+(15*(10)),BLACK,1.0);
NOBOLD g_printString("MOT PWM TPS ENCODER VAL",oriX+5, oriY+330,TEAL,1.0);
for(i=0;i<4;i++){
actuators[i][0]=48+i;
actuators[i][1]=':';
actuators[i][2]=' ';
NOBOLD g_printString(actuators[i],oriX+5, oriY+345+(15*(i)),WHITE,1.0);
convertNumToString(_bob.motor_pwm[i],4,*actuators,25,i,2);
convertNumToString(_bob.motor_tps[i],4,*actuators,25,i,7);
convertNumToString(_bob.motor_counter[i],10,*actuators,25,i,12);
NOBOLD g_printString(actuators[i],oriX+5, oriY+345+(15*(i)),BLACK,1.0);
}
if(_bob.enable_servos){
NOBOLD g_printString("SERVO TARGET DISABLED",oriX+5, oriY+410,WHITE,1.0);
NOBOLD g_printString("SERVO TARGET ENABLED",oriX+5, oriY+410,TEAL,1.0);
}
else {
NOBOLD g_printString("SERVO TARGET ENABLED",oriX+5, oriY+410,WHITE,1.0);
NOBOLD g_printString("SERVO TARGET DISABLED",oriX+5, oriY+410,TEAL,1.0);
}
for(i=0;i<4;i++){
servos[i][2]=49+i;//servos are 1 indexed!!
servos[i][3]=':';
servos[i][4]=' ';
NOBOLD g_printString(servos[i],oriX+5, oriY+425+(15*(i)),WHITE,1.0);
convertNumToString(_bob.servo_targets[i],4,*servos,25,i,6);
NOBOLD g_printString(servos[i],oriX+5, oriY+425+(15*(i)),BLACK,1.0);
}
}
int main()
{
printf("Press A for penis, B for v****a, or black button for titties!\n");
while(a_button() == 0 || b_button() == 0 || black_button() == 0)
{
if(a_button() == 1)
{
strategy = 1;
}
if(b_button() == 1)
{
strategy = 2;
}
if(black_button() == 1)
{
strategy = 3;
}
}
sleep(0.5);
if (strategy == 1)
{
printf("You picked penis as your first strategy, press B for v****a or black button for titties next\n");
while(b_button() == 0 || black_button() == 0)
{
if (b_button() == 1)
{
strategy = strategy * 10 + 2;
}
if (black_button() == 1)
{
strategy = strategy * 10 + 3;
}
}
}
if (strategy == 2)
{
printf("You picked v****a as your first strategy, press A for penis or black button for titties next\n");
while(a_button() == 0 || black_button() == 0)
{
if (a_button() == 1)
{
strategy = strategy * 10 + 1;
}
if (black_button() == 1)
{
strategy = strategy * 10 + 3;
}
}
}
if (strategy == 3)
{
printf("You picked titties as your first strategy, press A for penis or B for v****a next\n");
while(a_button() == 0 || b_button() == 0)
{
if (a_button() == 1)
{
strategy = strategy * 10 + 1;
}
if (b_button() == 1)
{
strategy = strategy * 10 + 2;
}
}
}
if (strategy - 1 / 10 == 2)
{
strategy = 213;
}
//Someone else can finish the rest :l
}
