int main()
{
int lspeed = 10;
int hspeed = 80;
camera_open(LOW_RES);
while (side_button() == 0)
{
camera_update();
if (get_object_center(0 , 0).x < 45)
{
motor(1 , lspeed);
motor(3 , hspeed);
printf("LEFT\n");
}
if (get_object_center(0 , 0).x > 115)
{
motor(1 , hspeed);
motor(3 , lspeed);
printf("RIGHT\n");
}
if (get_object_center(0 , 0).x >= 45 && get_object_center(0 , 0).x <= 115)
{
motor(1 , hspeed);
motor(3 , hspeed);
printf("CENTER\n");
}
}
return 0;
}
int check_stopping_event(int stopping_event) {
if (stopping_event == STOP_BY_ET) {
if (analog_et(ET_SENSOR) < 300) {
return 1;
}
} else if (stopping_event == STOP_BY_TOPHAT) {
display_printf(0, 0, "%4i", analog(L_TOPHAT));
if (analog(L_TOPHAT) > 750 || analog(R_TOPHAT) > 750) {
return 1;
}
} else if (stopping_event == STOP_BY_CAMERA) {
camera_update();
if (get_object_count(0) < 1) {
display_printf(0, 0, "No Object Found");
return 0;
}
if (get_object_area(0, 0) > PINGPONG_THRESHOLD) {
display_clear();
display_printf(0, 0, "Object Seen!");
if (get_object_center(0, 0).x > 90 && get_object_center(0, 0).x < 110) {
display_printf(0, 1, "Object centered");
return 1;
}
}
} else {
printf("Stopping type is not defined!\n");
return -1;
}
return 0;
}
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();
}
void found_something(){
ao();
camera_update();
msleep(100);
camera_update();
msleep(100);
camera_update();
msleep(100);
while(get_object_bbox(0,0).width < 80){
camera_update();
if(get_object_center(0,0).x > 120)
{
motor(Motor_Left,Drivespeed_middle/2);
motor(Motor_Right,-Drivespeed_middle/2);
camera_update();
} else if(get_object_center(0,0).x < 70) {
motor(Motor_Left,-Drivespeed_middle/2);
motor(Motor_Right,Drivespeed_middle/2);
camera_update();
} else {
motor(Motor_Left,Drivespeed_middle/2);
motor(Motor_Right,Drivespeed_middle/2);
camera_update();
}
msleep(5);
}
freeze(Motor_Left);
freeze(Motor_Right);
claw_close();
msleep(1000);
claw_up();
}
//routine to use the camera and see blob position based on standard create start routine
void calibrateCamera() {
int channel = 0;
while (a_button_clicked() == 0) {
msleep(2000);
camera_update();
printf ("=> object count %d \n", get_object_count(channel));
int i = 0;
int shelfPlaces[8] = {0,0,0,0,0,0,0,0};
while (i < get_object_count(channel)) {
printf("obj %d, ctr loc, area, shelf plcmt: %d, %d, %d\n", i, get_object_center(channel, i).x, get_object_area(channel, i),
getShelfPlacement(get_object_center(channel, i).x));
shelfPlaces[getShelfPlacement(get_object_center(channel, i).x)] += get_object_area(channel, i);
i++;
}
printf("==========================\n");
printf("=RESULTS =\n");
printf("==========================\n");
i = 1;
while (i < 8) {
printf ("shelfPlace : %d, total cube area : %d\n", i, shelfPlaces[i]);
i++;
}
printf("==========================\n");
printf("click b button for new camera capture, a to exit.\n");
while (b_button_clicked() == 0 && a_button_clicked() == 0 ) {
msleep(25);
}
}
printf("done with calibration function.\n");
}
void findBall(int color)
{
camera_update();
display_clear();
int count = 1;
int objNum = get_object_count(color);
while(objNum == 0)
{
camera_update();
objNum = get_object_count(color);
}
printf("%d time finding a ball", count);
point2 objCen = get_object_center(color, 0);
int errorX = 0, errorY = 0;
errorX = OFFSET_X - objCen.x;
errorY = OFFSET_Y - objCen.y;
while(!(a_button_clicked()))
{
if(BALL_NUM_BOX >= 2)
break;
if(errorX > -4 && errorX < 4 && errorY > -4 && errorY < 4)
break;
int turnLM = -1 * errorX * P_X + errorY * P_Y;
int turnRM = errorX * P_X + errorY * P_Y;
if(turnLM > -1 * MINS && turnLM < 0)
turnLM = -1 * MINS;
if(turnLM > 0 && turnLM < MINS)
turnLM = MINS;
if(turnRM > -1 * MINS && turnRM < 0)
turnRM = -1 * MINS;
if(turnRM > 0 && turnRM < MINS)
turnRM = MINS;
motor(LM, turnLM);
motor(RM, turnRM);
camera_update();
int objArea = get_object_area(color, 0);
while(objArea < 200)
{
ao();
camera_update();
objArea = get_object_area(color, 0);
}
count++;
display_clear();
printf("%d time finding a ball ", count);
printf("%d NUM", BALL_NUM_BOX);
objCen = get_object_center(color, 0);
errorX = OFFSET_X - objCen.x;
errorY = OFFSET_Y - objCen.y;
}
ao();
catchBalls(color);
}
//Precondition: camera must already be open
int center_on_blob(int desired_x, int color,float time ) {
int actual_x;
while (1) {
camera_update();
actual_x = get_object_center(color, 0).x;
printf("%i\n");
if (actual_x > desired_x) {
RIGHT(-20);
LEFT(20);
msleep(40);
LFREEZE();
RFREEZE();
}
if (actual_x < desired_x) {
RIGHT(20);
LEFT(-20);
msleep(40);
LFREEZE();
RFREEZE();
}
if (actual_x < desired_x+3 && actual_x > desired_x-3){
LFREEZE();
RFREEZE();
break;
}
msleep(200);
}
}
int main()
{
//while the a button isn't pressed
int x, y, color=0; //sets all variables to 0
camera_open(LOW_RES);
while(side_button()==0)
{
//sets camera to lowest resolution for minimal lag
//camera takes a picture
if(get_object_count(color) > 0) // If the right color is detected, run this code
{
x = get_object_center(color,0).x;
y = get_object_center(color,0).y; //x and y variables now represent x and y coordinates of the biggest color blob
if(x>80) //if the x coordinate is on the right
{
printf("The biggest blob is on the right and the coordinates are (%d,%d)", x ,y); //prints the words
}
if(x<80) //if x is on th left
{
printf("The biggest blob is on the left and the coordinates are (%d,%d)" , x ,y); //print the words
}
else
{
printf("no object in sight");
}
camera_update();
}
}
}
int main(){
camera_open(LOW_RES);
camera_load_config("color.config");
while(!(a_button_clicked())){
display_clear();
camera_update();
int objNum = get_object_count(GREEN);
if(objNum == 0)
printf("NO SUCH OBJECT!!!!!!!!");
else if(objNum > 0){
point2 objCen = get_object_center(GREEN, 0);
int objArea = get_object_area(GREEN, 0);
printf("X:\t%d\nY:\t%d\n", objCen.x, objCen.y);
printf("Area:\t%d\n", objArea);
}
msleep(100);
}
return 0;
}
int main(){
armUp();
clawOpen();
camera_open(LOW_RES);
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){
break;
}else{
stop(0.1);
}
}
while((seconds()-start_wait)<=60){
int area=get_object_area(chan, 0);
if(area>=600){
int i=0;
while(i<10){
camera_update();
i++;
}
int x=get_object_center(chan, 0).x;
if(x<65){
rightF(0.1, 100, 80);
}else{
leftF(0.1, 100, 80);
}
}else{
break;
}
}
armDown();
clawClose();
armUp();
}
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 foundit(){
printf("foundit \n");
int x;
camera_update();
if(get_object_count(green)>0){
while(get_object_center(green,0).y < 100){ //solange bis blob nahe genug
camera_update();
if(get_object_count(green)>0){ //schaut ob Blob noch in der Kamera
camera_update();
x = get_object_center(green,0).x;
if (x > 120) { //wenn Blob rechts--> rechts drehen ....
turn_right_speed(50); //je näher er dem pom kommt umso langsamer dreht er sich
} else if (x < 40) {
turn_left_speed(50); //je näher er dem pom kommt umso langsamer dreht er sich
} else if ((x > 40)||(x < 120)) {
forward();
if(analog10(LEFT_SENSOR)>left_blk-150||analog10(RIGHT_SENSOR)>right_blk-150){
foundOutArea=1;
printf("drausen\n");
}
}
}
else{ //wenn blob nicht mehr in Kamera wieder in grobjustierung(takepom) gehen
return 0;
}
}
printf("target aquired! \n");
set_servo_position(CLAW1, CLAW1_FRONT);
set_servo_position(CLAW2, CLAW2_FRONT);
forward(); //etwas vor fahren um Blob sicher zu erwischen
int time=200;
while((analog10(ET)<310&&!digital(15)&&!digital(14))&&time>=0 ){
msleep(5);
time--;
}
printf("%i",digital(15));
printf("\nLast time ");
printf("%i",time);
stop();
msleep(100);
if(time>=0){ //fals er angefahren ist
back();
msleep(150);
motor(RIGHT_MOTOR,TURN_SPEED);
motor(LEFT_MOTOR,0);
msleep(1500);
claw_close();
}
claw_close();
if(foundOutArea){ //wenn ers auserhalb des Schwarzen bereichs gefunden hat
printf("zurück fahren\n");
back();
while(analog10(LEFT_SENSOR)>left_blk-150||analog10(RIGHT_SENSOR)>right_blk-150){}
msleep(700);
}
back();
msleep(800);
if(analog10(LEFT_SENSOR)>left_blk-150||analog10(RIGHT_SENSOR)>right_blk-150){ //wenn er so weit zurück sein sollte das er auf schwarz is
forward();
while(analog10(LEFT_SENSOR)<left_blk-150&&analog10(RIGHT_SENSOR)<right_blk-150){} //wieder forwärts bis er weis is damits funktioniert
}
camera_update();
return 1;
}
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)
{
}
}
int main()
{
int stupid_int = 0;
int value = 0;
int i = 0;
int angle_of_pole = 90;
camera_open(LOW_RES);
camera_update();
create_connect();
set_create_total_angle(0);
value = get_create_total_angle();
printf("Angle is %d\n", value);
//------------------------
// part 1: spin until past all booster sections
while(get_create_total_angle() > -85){
create_spin_CW(75);
}
//---------------------
//end part 1
camera_update();
sleep(2);
point2 mcenter = get_object_center(1,0);
set_servo_position(0,OPEN_POSITION); //this is opening the claw so it won't go to a random position when enabling the servos
enable_servos();
//--------------------------------------
// part 2: find booster section using function i wrote at bottom of code then line up with B2(pole)
find_item();
printf("I saw a booster section!\n");
while(digital(10) == 0) {
create_drive_straight(-50);
if(digital(10) == 1){
create_stop();
}
}
while(stupid_int == 0) {
if (digital(10) == 1){
set_servo_position(0,CLOSE_POSITION_FOR_BOOSTER_SECTIONS);
stupid_int == 1;
msleep(200);
printf("I have grabbed a booster section!\n");
}
}
move_to_position(0, -900, UPPOSITION);
set_create_total_angle(0);
while(get_create_total_angle() < angle_of_pole){
create_spin_CCW(75);
}
//---------------------
//end part 2
set_create_total_angle(0);
// part 3: go forward until i find B2 (not complete)
set_servo_position(1,OPEN_POSITION);
angle_of_pole = 60;
//end part 3
//part 4: repeat everything
/*while(get_create_total_angle() > -80){
create_spin_CW(75);
}
//---------------------
//end part 1
camera_update();
sleep(2);
point2 mcenter = get_object_center(0,0);
set_servo_position(0,OPEN_POSITION); //this is opening the claw so it won't go to a random position when enabling the servos
//--------------------------------------
// part 2: find booster section using function i wrote at bottom of code then line up with B2(pole)
find_item();
printf("I saw a booster section!\n");
while(digital(10) == 0) {
create_drive_straight(-50);
if(digital(10) == 1){
create_stop();
}
}
while(stupid_int == 0) {
if (digital(10) == 1){
set_servo_position(0,CLOSE_POSITION_FOR_BOOSTER_SECTIONS);
stupid_int == 1;
msleep(200);
printf("I have grabbed a booster section!\n");
}
}
move_to_position(0, -900, UPPOSITION);
set_create_total_angle(0);
while(get_create_total_angle() < angle_of_pole){
create_spin_CCW(75);
}
//---------------------
//end part 2
set_create_total_angle(0);
// part 3: go forward until i find B2 (not complete)
set_servo_position(1,OPEN_POSITION);
angle_of pole = 50;
//end part 3
while(get_create_total_angle() > -70){
create_spin_CW(75);
}
//---------------------
//end part 1
camera_update();
sleep(2);
point2 mcenter = get_object_center(0,0);
set_servo_position(0,OPEN_POSITION); //this is opening the claw so it won't go to a random position when enabling the servos
//--------------------------------------
// part 2: find booster section using function i wrote at bottom of code then line up with B2(pole)
find_item();
printf("I saw a booster section!\n");
while(digital(10) == 0) {
create_drive_straight(-50);
if(digital(10) == 1){
create_stop();
}
}
while(stupid_int == 0) {
if (digital(10) == 1){
set_servo_position(0,CLOSE_POSITION_FOR_BOOSTER_SECTIONS);
stupid_int == 1;
msleep(200);
printf("I have grabbed a booster section!\n");
}
}
move_to_position(0, -900, UPPOSITION);
set_create_total_angle(0);
while(get_create_total_angle() < angle_of_pole){
create_spin_CCW(75);
}
//---------------------
//end part 2
set_create_total_angle(0);
// part 3: go forward until i find B2 (not complete)
set_servo_position(1,OPEN_POSITION);
angle_of pole = 50;
//end part 3
while(get_create_total_angle() > -100){
create_spin_CW(75);
}
//---------------------
//end part 1
camera_update();
sleep(2);
point2 mcenter = get_object_center(0,0);
set_servo_position(0,OPEN_POSITION); //this is opening the claw so it won't go to a random position when enabling the servos
//--------------------------------------
// part 2: find booster section using function i wrote at bottom of code then line up with B2(pole)
find_item();
printf("I saw a booster section!\n");
while(digital(10) == 0) {
create_drive_straight(-50);
if(digital(10) == 1){
create_stop();
}
}
while(stupid_int == 0) {
if (digital(10) == 1){
set_servo_position(0,CLOSE_POSITION_FOR_BOOSTER_SECTIONS);
stupid_int == 1;
msleep(200);
printf("I have grabbed a booster section!\n");
}
}
move_to_position(0, -900, UPPOSITION);
set_create_total_angle(0);
while(get_create_total_angle() < angle_of_pole){
create_spin_CCW(75);
}
//---------------------
//end part 2
set_create_total_angle(0);
// part 3: go forward until i find B2 (not complete)
set_servo_position(1,OPEN_POSITION);
angle_of pole = 40;
//end part 3
while(get_create_total_angle() > -60){
create_spin_CW(75);
}
//---------------------
//end part 1
camera_update();
sleep(2);
point2 mcenter = get_object_center(0,0);
set_servo_position(0,OPEN_POSITION); //this is opening the claw so it won't go to a random position when enabling the servos
//--------------------------------------
// part 2: find booster section using function i wrote at bottom of code then line up with B2(pole)
find_item();
printf("I saw a booster section!\n");
while(digital(10) == 0) {
create_drive_straight(-50);
if(digital(10) == 1){
create_stop();
}
}
while(stupid_int == 0) {
if (digital(10) == 1){
set_servo_position(0,CLOSE_POSITION_FOR_BOOSTER_SECTIONS);
stupid_int == 1;
msleep(200);
printf("I have grabbed a booster section!\n");
}
}
move_to_position(0, -900, UPPOSITION);
set_create_total_angle(0);
while(get_create_total_angle() < angle_of_pole){
create_spin_CCW(75);
}
//---------------------
//end part 2
set_create_total_angle(0);
// part 3: go forward until i find B2 (not complete)
set_servo_position(1,OPEN_POSITION);
angle_of pole = 60;
//end part 3*/
//end part 4
}
void findBall(){
display_clear();
int temptLMSpeed = LM_MINS; // changeable minimum speed for left motor
int temptRMSpeed = RM_MINS; // changeable minimum speed for right morot
int lastLMSpeed, lastRMSpeed; // motor speed
int lastLMPos, lastRMPos; // motor positions
int errorX = 0, errorY = 0; // difference of current ball coordinate and target coordinate
int objArea; // area of the largest object in sight
point2 objCen; // object center coordinate
camera_update();
int objNum = get_object_count(GREEN);
while(objNum == 0){
camera_update();
objNum = get_object_count(GREEN);
}
// find a ball in sight
objCen = get_object_center(GREEN, 0);
errorX = OFFSET_X - objCen.x;
errorY = OFFSET_Y - objCen.y;
while(!(a_button_clicked())){
camera_update();
objArea = get_object_area(GREEN, 0);
while(objArea < 200){
ao();
camera_update();
objArea = get_object_area(GREEN, 0);
}
// make sure really a ball is in sight
objCen = get_object_center(GREEN, 0);
errorX = OFFSET_X - objCen.x;
errorY = OFFSET_Y - objCen.y;
// find differences between coordinates
if(errorX > -3 && errorX < 2 && errorY > -3 && errorY < 2)
break;
int turnLM = -1 * errorX * P_X + errorY * P_Y;
int turnRM = errorX * P_X + errorY * P_Y;
// turn for LM, RM
if(turnLM == lastLMSpeed && lastLMPos == get_motor_position_counter(LM))
temptLMSpeed = temptLMSpeed + 1;
if(turnLM == lastLMSpeed && lastLMPos != get_motor_position_counter(LM))
temptLMSpeed = temptLMSpeed - 1;
if(turnLM == -1 * lastLMSpeed && lastLMPos != get_motor_position_counter(LM))
temptLMSpeed = temptLMSpeed - 2;
if(turnRM == lastRMSpeed && lastRMPos == get_motor_position_counter(RM))
temptRMSpeed = temptRMSpeed + 1;
if(turnRM == lastRMSpeed && lastRMPos != get_motor_position_counter(RM))
temptRMSpeed = temptRMSpeed - 1;
if(turnRM == -1 * lastRMSpeed && lastRMPos != get_motor_position_counter(RM))
temptRMSpeed = temptRMSpeed - 2;
lastLMSpeed = turnLM;
lastLMPos = get_motor_position_counter(LM);
lastRMSpeed = turnRM;
lastRMPos = get_motor_position_counter(RM);
// check if speed is too high or low, thus make modification to minimum speed
if(turnLM > -1 * temptLMSpeed && turnLM < 0)
turnLM = -1 * temptLMSpeed;
if(turnLM > 0 && turnLM < temptLMSpeed)
turnLM = temptLMSpeed;
if(turnRM > -1 * temptRMSpeed && turnRM < 0)
turnRM = -1 * temptRMSpeed;
if(turnRM > 0 && turnRM < temptRMSpeed)
turnRM = temptRMSpeed;
// set minimum speed: avoid unmoving
motor(LM, turnLM);
motor(RM, turnRM);
// move
}
ao();
catchBall(GREEN);
// stop and catch
}
int main(int argc, char *argv[])
{
int blob_count = 0, i = 0;
int main_channel = 2; /* red - botguy */
double time_of_snapshot = 0.0;
double start = 0.0;
printf("args: %d\n", argc);
if (argc > 1)
{
if (argv[1][0] == '0') main_channel = 0;
else if (argv[1][0] == '1') main_channel = 1;
else if (argv[1][0] == '2') main_channel = 2;
else if (argv[1][0] == '3') main_channel = 3;
}
for (i = 0; i < argc; i++) {
printf("arg[%d] = %s\n", i, argv[i]);
}
printf("\n");
int area[NUM_TRACKING];
point2 center[NUM_TRACKING];
rectangle blob[NUM_TRACKING];
//int area1 = 0, areaX = 0;
//point2 center;
init_grid();
//wait_for_light(0);
camera_open();
printf("Num channels: %d", get_channel_count());
camera_update();
time_of_snapshot = seconds();
msleep(300);
start = seconds();
do {
// Initialize
memset(center, 0, sizeof(center));
memset(area, 0, sizeof(area));
clear_grid();
// Begin
// Do not do anything with the camera until enough time has passed
if (seconds() - time_of_snapshot >= 0.1) {
camera_update();
time_of_snapshot = seconds();
blob_count = get_object_count(main_channel);
//sprintf(msg, "#:%2d", blob_count); diag();
printf("#:%-2d ", blob_count);
if (blob_count > count_of(area)) {
blob_count = count_of(area);
}
if (blob_count > 0) {
for (i = 0; i < blob_count; ++i) {
area[i] = get_object_area(main_channel, i);
center[i] = get_object_center(main_channel, i);
blob[i] = get_object_bbox(main_channel, i);
plot(&blob[i], i + '0');
}
for (i = 0; i < blob_count; ++i) {
printf(" %3d", blob[i].width * blob[i].height);
}
for (; i < count_of(blob); ++i) {
printf(" ");
}
printf(" ");
for (i = 0; i < blob_count; ++i) {
printf(" %2dx%2d", blob[i].width, blob[i].height);
}
//show_xy(center[0].x, center[0].y);
}
//printf(" |%3d %4d <-> %4d %3d| %s\n", delta_left, left, right, delta_right, message);
printf("%s\n", message);
message[0] = 0;
diag();
show_grid();
}
//printf("The time is: %f %f\n", seconds(), seconds() - start);
} while (seconds() - start <= 4.0);
camera_close();
return 0;
}
int offsetFromObject(int channel, int index){ //positive = Object to right; negative = Object to left; returns the distance from the center of the camera to the center of the object.
point2 objectCenter = get_object_center(channel, 0);
int output = -1*(CAMERA_WIDTH-(objectCenter.x)));
return output;
}
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;
}
inline void blob_update()
{
// set config file
camera_update();
//update green state
current.green.x = get_object_center(0 , 0).x;
current.green.y = get_object_center(0 , 0).y;
current.green.size = get_object_area(0 , 0);
//update orange state
current.orange.x = get_object_center(1 , 0).x;
current.orange.y = get_object_center(1 , 0).y;
current.orange.size = get_object_area(1 , 0);
//update red state
current.red.x = get_object_center(2 , 0).x;
current.red.y = get_object_center(2 , 0).y;
current.red.size = get_object_area(2 , 0);
// update botguy state
current.botguy.x = get_object_center(3 , 0).x;
current.botguy.y = get_object_center(3 , 0).y;
current.botguy.size = get_object_area(3 , 0);
// set config file
camera_update();
// update pink booster state
current.pink.x = get_object_center(0 , 0).x;
current.pink.y = get_object_center(0 , 0).y;
current.pink.size = get_object_area(0 , 0);
// update teal booster state
current.teal.x = get_object_center(1 , 0).x;
current.teal.y = get_object_center(1 , 0).y;
current.teal.size = get_object_area(1 , 0);
}
