// prepare:
void prepare(){
camera_open(LOW_RES);
camera_load_config(CONFIG);
camera_update();
camera_update();
camera_update();
// prepare camera
enable_servos();
set_servo_position(BSV, BSV_LEVEL);
set_servo_position(ASV, ASV_BACK);
set_servo_position(RSV, RSV_DOWN);
set_servo_position(SSV, SSV_BACK);
// prepare servos
set_analog_pullup(FSS, 0);
set_analog_pullup(BSS, 0);
// prepare sensors
display_clear();
printf("prepare succeeds");
while(digital(TSS) == 0);
msleep(1500);
//wait_for_light(SSS);
//shut_down_in(TIME_LIMIT);
// show controller that coke is ready
}
void freeze_halt()
{
ao();
for(int i = 0; i < 8; ++i) {
set_analog_pullup(i, 1);
}
for(int i = 8; i < 16; ++i) {
set_digital_output(i, 0);
set_digital_pullup(i, 1);
}
create_stop();
}
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();
}
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()
{
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 wait_for_light(int light_port_)
{
int xBut, l_on_, l_off_, l_mid_, t, OK = 0;
float s;
xBut = get_extra_buttons_visible();
set_extra_buttons_visible(0);
set_a_button_text("-");
set_c_button_text("-");
set_analog_pullup(light_port_, 1);
while (!OK) {
set_b_button_text("Light is On");
display_clear();
display_printf (0, 0, "CALIBRATE: sensor port #%d", light_port_);
display_printf(0, 1, " press button when light is on");
while(b_button_clicked() == 0) {
l_on_ = analog10 (light_port_);
display_printf(0,1," light on value is = %d ", l_on_);
msleep(50);
}
l_on_ = analog10(light_port_); /* sensor value when light is on */
set_b_button_text("Light is Off");
display_printf(0,1," light on value is = %d ", l_on_);
msleep(200);
beep();
display_printf(0,2," press button when light off");
while(b_button_clicked() == 0) {
l_off_ = analog10(light_port_);
display_printf(0,3," light off value is = %d ", l_off_);
msleep(50);
}
l_off_ = analog10(light_port_); /* sensor value when light is off */
display_printf(0,3," light off value is = %d ", l_off_);
msleep(200);
if((l_off_ - l_on_) >= 60) { /* bright = small values */
OK = 1;
l_mid_ = (l_on_ + l_off_) / 2;
display_printf(0, 5, "Good Calibration!");
display_printf(0, 7, "Diff = %d: WAITING", l_off_ - l_on_);
while(analog10(light_port_) > l_mid_);
} else {
s = seconds();
display_printf(0,7,"BAD CALIBRATION");
if(l_off_ < 512){
display_printf(0,8," Add Shielding!!");
msleep(5000);
} else {
display_printf(0,8," Aim sensor!!");
msleep(5000);
}
}
}
set_extra_buttons_visible(xBut);
set_a_button_text("A");
set_b_button_text("B");
set_c_button_text("C");
}