/*
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;
}
