/* 1 dist unit = 1 second at speed 100 with standard wheels; */ int testTurnSpeed(){ //account for the distance between the wheels, etc. Returns a "dist" number which represents a 360 degree turn. Speed of turn * length of turn = value returned. //Thus, it takes X number of units (X determined here), to turn 360 degrees. int currentPosition = 0; int FIRST_SPEED = 250; //speed at which for the first portion. int TIMEINTERVAL = 40; printf("Please press A when the Robot has turned 90 degrees. \n"); while(a_button_clicked()==0){ //turn a bit past 90 degrees. turnRight(FIRST_SPEED, TIMEINTERVAL); currentPosition = currentPosition + (FIRST_SPEED*TIMEINTERVAL); } printf("Moving back a bit. \n"); int toMoveBack = currentPosition/5; //move back 1/5 of the distance. turnLeft(FIRST_SPEED, toMoveBack/FIRST_SPEED); printf("Please press A when the Robot is back at 90 degrees. \n"); while(a_button_clicked()==0){ //turn to 90 degrees at 1/4 previous speed. turnRight(FIRST_SPEED/4, TIMEINTERVAL); currentPosition = currentPosition + (FIRST_SPEED/4*TIMEINTERVAL); } printf("Current Value: %d \n Will now turn 360 degrees. \n", currentPosition*4); turnRight(500, currentPosition*4/500); //make 360 degree turn at speed 300. printf("Button A: Done \n Button B: Redo \n"); int button = getFirstButtonPressed(); if(button == 0){ printf("360 degree turn value: %d \n", (currentPosition*4)); } else if(button == 1){ return testTurnSpeed(); } else return -1; }
//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"); }
int main() { printf("version 1.0 "); printf("moving forwrd 10 "); moveForward(10); printf("right angle right"); rightAngleFwd(RIGHT); moveForward(10); printf("click a button to continue"); while (a_button_clicked() == 0) { msleep(25); } printf("moving bckwrd 10 "); moveBackward(10); printf("right angle back right"); rightAngleBwd(RIGHT); moveBackward(10); printf("click a button to continue"); while (a_button_clicked() == 0) { msleep(25); } //printf("program finished, POMS should be in...\n"); return 0; }
// test: void TEST() { ao(); display_clear(); printf("next step ready"); while(!a_button_clicked()); }
int main() { create_connect(); create_full(); while(digital(15) == 0); create_drive_direct(-1500, -1500); while(!(a_button_clicked())); return 0; }
int main() { create_connect(); create_full(); while(!(a_button_clicked())){ display_clear(); printf("cw:%d\nlw:%d\nrw:%d\n", get_create_cwdrop(), get_create_lwdrop(), get_create_rwdrop()); msleep(20); } return 0; }
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); }
int main () { camera_open(); depth_open(); while(!(a_button_clicked())){ camera_update(); depth_update(); display_clear(); printf("prepare succeeds"); } return 0; }
int getFirstButtonPressed(){ //returns 0 for A, 1 for B, 2 for C. while(1==1){ if(a_button_clicked()==1){ return 0; } if(b_button_clicked()==1){ return 1; } if(c_button_clicked()==1){ return 2; } msleep(40); } }
void prepare() { camera_open(LOW_RES); camera_load_config(CONFIG); camera_update(); 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); display_clear(); printf("prepare succeeds"); while(!(a_button_clicked())); }
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 get_pot_reading(int port, int min, int max) { int pot_reading; int actual_reading; int previous_reading = -1; while (1) { pot_reading = analog10(port); actual_reading = (pot_reading / 1023.0) * (max - min) + min + 0.5; if (actual_reading != previous_reading) { printf("%i\n", actual_reading); } previous_reading = actual_reading; if (a_button_clicked()) { msleep(500); return actual_reading; } } }
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 m1=0,m2=3,pageno=0; double createDriveTime; printf("Start!\n"); extra_buttons_show(); while (1) { while (pageno == 0) { set_a_button_text("Forward"); set_b_button_text("Backward"); set_c_button_text("All Off"); set_x_button_text("OpenHand"); set_y_button_text("CloseHand"); set_z_button_text("Create Page"); if (a_button_clicked()) { printf("Moving forward...\n"); motor(m1,100); motor(m2,100); while (!c_button_clicked()) { msleep(1); } ao(); } if (b_button_clicked()) { printf("Moving backward...\n"); motor(m1,-100); motor(m2,-100); while (!c_button_clicked()) { msleep(1); } ao(); } if (x_button_clicked()) { enable_servo(3); set_servo_position(3,0); msleep(300); disable_servo(3); } if (y_button_clicked()) { enable_servo(3); set_servo_position(3,1300); msleep(300); disable_servo(3); } if (z_button_clicked()) { pageno=1; create_connect(); } } while (pageno == 1) { set_a_button_text("Create Fwd"); set_b_button_text("Create Bwd"); set_c_button_text("Stop"); set_x_button_text("Turn R"); set_y_button_text("Turn L"); set_z_button_text("M S Page"); if (a_button_clicked()) { createDriveTime = 0; printf("Create Moving forward w/ spd 100...\n"); create_drive_straight(100); createDriveTime = seconds(); while (!c_button_clicked()) { msleep(1); } create_stop(); printf("Time: %f seconds\n",seconds() - createDriveTime ); } if (b_button_clicked()) { createDriveTime = 0; printf("Create Moving backward w/ spd 100...\n"); create_drive_straight(-100); createDriveTime = seconds(); while (!c_button_clicked()) { msleep(1); } create_stop(); printf("Time: %f seconds\n",seconds() - createDriveTime ); } if (x_button_clicked()) { create_drive(-50,0); while (!c_button_clicked()) { msleep(1); } create_stop(); } if (y_button_clicked()) { create_drive(50,0); while (!c_button_clicked()) { msleep(1); } create_stop(); } if (z_button_clicked()) { pageno=0; create_disconnect(); } } } return 0; }