void bringback2cube(){
//claw up
motor(Motor_Up,Motor_up_speed);
//zurück 1s
drive(1000,-Drivespeed,-Drivespeed);
//turn right 90
drive(970,Turnspeed,-Turnspeed);
//vor to calibrate
drive(1000,Drivespeed,Drivespeed);
//back
drive(500,-Drivespeed,-Drivespeed);
//turn so little bit b4 cubes
drive(1130,Turnspeed,-Turnspeed);
//vor
drive(900,Drivespeed,Drivespeed);
//wait for claw up
claw_up();
//camera fix
camera_update();
//hide your cubes
cube_is_near();
//back
drive(1000,-Drivespeed,-Drivespeed);
//turn left more than 90 idk
drive(1330,-Turnspeed,Turnspeed);
//start to down motor
motor(Motor_Up,Motor_down_speed);
//vor to calibrate
drive(1000,Drivespeed,Drivespeed);
//langsamer weil wackeldackel
drive(2000,Drivespeed_middle+20,Drivespeed_middle+20);
//back
drive(400,-Drivespeed,-Drivespeed);
//turn more than 90 lulz
drive(980,-Turnspeed,Turnspeed);
//light left and shit
drive(5000,Drivespeed_middle*2,(Drivespeed_middle*2)-7);
//light back and shit
drive(1000,-Drivespeed_middle*2,(-Drivespeed_middle*2)+7);
//wait for claw down
while(!digital(Sensor_Down)){
if(seconds()>start+113){
claw_open();
disable_servos();
}
}
freeze(Motor_Up);
msleep(2000);
claw_open();
disable_servos();
}
void cube_is_near(){
camera_update();
msleep(500);
camera_update();
enable_servos();
claw_open();
printf("cube is near");
freeze(Motor_Left);
freeze(Motor_Right);
if(get_object_count(0)!=0){
printf("gleich gefunden\n");
if(get_object_bbox(0,0).width > 20){
printf("groß genug");
funden=1;
found_something();
}
}
double s=seconds();
motor(Motor_Right,30);
while(funden==0){//nichts gefunden
camera_update();
printf("nach rechts\n");
if(get_object_count!=0){
printf("1 gefunden \n");
if(get_object_bbox(0,0).width > 20){
printf("groß genug");
found_something();
funden=1;
break;
}
}
if(seconds()>s+2){
printf("zeit rechts\n");
freeze(Motor_Right);
break;
}
}
s=seconds();
motor(Motor_Left,30);
while(funden==0){
camera_update();
printf("nach links drehen \n");
if(get_object_count!=0){
printf("1 gefunden \n");
if(get_object_bbox(0,0).width > 20){
printf("groß genug");
found_something();
funden=1;
break;
}
}
if(seconds()>s+4){
printf("zeit links\n");
freeze(Motor_Left);
break;
}
}
}
void bringback(){
//zurück zur linie
drive_till_line_backward();
//vor damit nicht insert into create
drive(1200,Drivespeed_middle,Drivespeed_middle);
//turn left 90
//drive(1100,-1*Drivespeed_middle-20,Drivespeed_middle+20);
motor(Motor_Left,-Drivespeed_middle*2);
while(analog(Sensor_Line_Left) < Sensor_Black){}
msleep(400);
while(analog(Sensor_Line_Left) > Sensor_Black){}
msleep(100);
freeze(Motor_Left);
//start claw down
motor(Motor_Up,Motor_down_speed);
//gerade but leicht links so it twerks
drive(9000,Drivespeed,Drivespeed);
wisch();
//vor to calibrate
drive(5000,Drivespeed_middle+20,Drivespeed_middle+20);
//back
drive(400,-Drivespeed,-Drivespeed);
//turn more than left 90
drive(980,-Turnspeed,Turnspeed);
//leicht rechts to calibrate
drive(5000,Drivespeed_middle*2,(Drivespeed_middle*2)-7);
//leicht back rechts to calibrate && würfel fit in tube
drive(800,-Drivespeed_middle*2,(-Drivespeed_middle*2)+7);
//wait for claw down
while(!digital(Sensor_Down)){}
freeze(Motor_Up);
msleep(2000);
claw_open();
msleep(2000);
}
void put_out(){
printf("put out \n");
set_servo_position(CAM_SERVO,CAM_SERVO_POSITION_RIGHT);
forward();
msleep(1300);
claw_open();
back();
msleep(1800);
while(analog10(LEFT_SENSOR)>=left_blk-150&&analog10(RIGHT_SENSOR)>=right_blk-150){}
claw_close();
forward();
msleep(2100);
back();
msleep(1100);
find_line_position_again();
set_servo_position(CAM_SERVO,CAM_SERVO_POSITION_RIGHT);
camera_update();
}
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;
}
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() {
//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;
}
void claw_down(){//claw down and open
claw_open();
servo_set(SERV_WRIST,1650,.7);
}
