void Right_AngleMAV(int Direction, double timeLimit){ long rightClicksRight=0.6*TICKS_PER_REVOLUTION_RIGHT; long leftClicksRight=0.6*TICKS_PER_REVOLUTION_LEFT; clear_motor_position_counter(LEFT_PORT_WHEEL); clear_motor_position_counter(RIGHT_PORT_WHEEL); double startTURN90Time = seconds(); while((seconds() - startTURN90Time <= timeLimit) && (get_motor_position_counter(LEFT_PORT_WHEEL) < leftClicksRight) && (get_motor_position_counter(LEFT_PORT_WHEEL) > -leftClicksRight)){ if(Direction==RIGHT){ mrp(RIGHT_PORT_WHEEL, (int)CRUISING_SPEED_TURNS_RIGHT, -rightClicksRight); mrp(LEFT_PORT_WHEEL, (int)CRUISING_SPEED_TURNS_LEFT, leftClicksRight); bmd(RIGHT_PORT_WHEEL); bmd(LEFT_PORT_WHEEL); } else if(Direction==LEFT){ mrp(RIGHT_PORT_WHEEL, (int)CRUISING_SPEED_TURNS_RIGHT, rightClicksRight); mrp(LEFT_PORT_WHEEL, (int)CRUISING_SPEED_TURNS_LEFT, -leftClicksRight); bmd(RIGHT_PORT_WHEEL); bmd(LEFT_PORT_WHEEL); } else{ printf("Invalid input. LEFT and RIGHT globals accepted only\n"); } }//Close While }
void Forty_Five_Degree_AngleMAV(int Direction, double timeLimit){ long rightClicksRight=0.35*TICKS_PER_REVOLUTION_RIGHT; //39 long leftClicksRight=0.35*TICKS_PER_REVOLUTION_LEFT; clear_motor_position_counter(LEFT_PORT_WHEEL); clear_motor_position_counter(RIGHT_PORT_WHEEL); if(Direction==RIGHT){ double startTURN45Time = seconds(); while((seconds() - startTURN45Time <= timeLimit) && ((get_motor_position_counter(LEFT_PORT_WHEEL) < leftClicksRight) && (get_motor_position_counter(RIGHT_PORT_WHEEL) > -rightClicksRight))){ mav(RIGHT_PORT_WHEEL, -CRUISING_SPEED_TURNS); mav(LEFT_PORT_WHEEL, CRUISING_SPEED_TURNS); msleep(35); } } else if(Direction==LEFT){ double startTURN45Time = seconds(); while((seconds() - startTURN45Time <= timeLimit) && ((get_motor_position_counter(RIGHT_PORT_WHEEL) < rightClicksRight) && (get_motor_position_counter(LEFT_PORT_WHEEL) > -leftClicksRight))){ mav(RIGHT_PORT_WHEEL, CRUISING_SPEED_TURNS); mav(LEFT_PORT_WHEEL, -CRUISING_SPEED_TURNS); msleep(35); } } else{ printf("Invalid input. LEFT and RIGHT globals accepted only\n"); } }
void turnRight(){ clear_motor_position_counter(LM); clear_motor_position_counter(RM); mtp(LM, 1500,HALF_CIRCLE); mtp(RM, -1500, -1 * HALF_CIRCLE); bmd(LM); bmd(RM); }
void turnBack(){ clear_motor_position_counter(LM); clear_motor_position_counter(RM); mtp(LM, 1500, ONE_CIRCLE); mtp(RM, -1500, -1 * ONE_CIRCLE); bmd(LM); bmd(RM); }
// move: void moveStraight(int distance) { clear_motor_position_counter(c_ileftMotor); clear_motor_position_counter(c_irightMotor); mtp(c_ileftMotor, c_iforwardSpeed, distance); mtp(c_irightMotor, c_iforwardSpeed, distance); bmd(c_ileftMotor); bmd(c_irightMotor); }
void lego_drive_until_black_line(int direction, int speed, int line_size) { int left_ticks_traveled = 0; int right_ticks_traveled = 0; float right_error; float left_error; float desired_value; int left_threshold; int right_threshold; int right_sensor_value; int left_sensor_value; int left_multiplier; int right_multiplier; if (line_size == SMALL_LINE) { left_threshold = LEFT_TOPHAT_SMALL_BLACK; right_threshold = RIGHT_TOPHAT_SMALL_BLACK; } else { left_threshold = LEFT_TOPHAT_LARGE_BLACK; right_threshold = RIGHT_TOPHAT_LARGE_BLACK; } clear_motor_position_counter(LEFT_MOTOR); clear_motor_position_counter(RIGHT_MOTOR); motor(LEFT_MOTOR, speed * direction); motor(RIGHT_MOTOR, speed * direction); while(TRUE) { left_sensor_value = analog(LEFT_TOPHAT); right_sensor_value = analog(RIGHT_TOPHAT); if (left_sensor_value > left_threshold || right_sensor_value > right_threshold) { break; } left_ticks_traveled = get_motor_position_counter(LEFT_MOTOR); right_ticks_traveled = get_motor_position_counter(RIGHT_MOTOR); desired_value = (left_ticks_traveled + right_ticks_traveled) / 2.0; left_error = desired_value - left_ticks_traveled; right_error = desired_value - right_ticks_traveled; left_multiplier = (int) ((left_error * l_kP) + 0.5); right_multiplier = (int) ((right_error * r_kP) + 0.5); motor(LEFT_MOTOR, (speed * direction) + left_multiplier); motor(RIGHT_MOTOR, (speed * direction) + right_multiplier); } freeze(LEFT_MOTOR); freeze(RIGHT_MOTOR); }
void collectBall(){ motor(FM, FM_COLLECT); clear_motor_position_counter(LM); clear_motor_position_counter(RM); mtp(LM, LM_SP_STR2, 800); mtp(RM, RM_SP_STR2, 800); bmd(LM); bmd(RM); motor(FM, 0); // move and collect rest balls }
void collectBall(){ motor(FM, FM_COLLECT); clear_motor_position_counter(LM); clear_motor_position_counter(RM); mtp(LM, 1500, 1000); mtp(RM, 1500, 1000); bmd(LM); bmd(RM); motor(FM, 0); msleep(2500); // move and collect rest balls }
int main(int argc, char** argv) { clear_motor_position_counter(1); clear_motor_position_counter(2); motor(1, 30); motor(2, 30); while (!side_button()) { printf("pos: %i/%i\n", get_motor_position_counter(1), get_motor_position_counter(2)); msleep(10); } alloff(); return 0; }
void lego_drive_centimeters(float centimeters, int direction, int speed) { int left_ticks_traveled = 0; int right_ticks_traveled = 0; int ticks = (int) (centimeters * TICKS_PER_CENTIMETER); float desired_value; int left_multiplier; int right_multiplier; error current_error, previous_error, error_delta, errors_sum; clear_motor_position_counter(LEFT_MOTOR); clear_motor_position_counter(RIGHT_MOTOR); previous_error.left = 0; previous_error.right = 0; errors_sum.left = 0; errors_sum.right = 0; motor(LEFT_MOTOR, speed * direction); motor(RIGHT_MOTOR, speed * direction); while(TRUE) { if (abs(left_ticks_traveled) >= ticks || abs(right_ticks_traveled) >= ticks) { break; } left_ticks_traveled = get_motor_position_counter(LEFT_MOTOR); right_ticks_traveled = get_motor_position_counter(RIGHT_MOTOR); desired_value = (left_ticks_traveled + right_ticks_traveled) / 2.0; current_error.left = desired_value - left_ticks_traveled; current_error.right = desired_value - right_ticks_traveled; error_delta.left = current_error.left - previous_error.left; error_delta.right = current_error.right - previous_error.right; errors_sum.left = (errors_sum.left * 0.7) + current_error.left; errors_sum.right = (errors_sum.right * 0.7) + current_error.right; left_multiplier = (int) (((current_error.left * l_kP) + 0.5) + ((errors_sum.left * l_kI) + 0.5) + ((error_delta.left * l_kD) + 0.5)); right_multiplier = (int) (((current_error.right * r_kP) + 0.5) + ((errors_sum.right * r_kI) + 0.5) + ((error_delta.right * r_kD) + 0.5)); motor(LEFT_MOTOR, (speed * direction) + left_multiplier); motor(RIGHT_MOTOR, (speed * direction) + right_multiplier); previous_error = current_error; } freeze(LEFT_MOTOR); freeze(RIGHT_MOTOR); msleep(100); }
void _arm_hold() { clear_motor_position_counter(1); clear_motor_position_counter(3); while (1) { mav(1, 100); mav(3, 100); msleep(1); mav(1, -100); mav(3, -100); msleep(1); } }
void release() { // open claw until touch sensor #14 is activated mav(3, -400); while(digital(14) ==0) { } off(3); clear_motor_position_counter(3); }
void servo_booster(int servo_port, int motor_port, int servo_position, int motor_position) { clear_motor_position_counter(motor_port); motor(motor_port, 100); set_servo_position(servo_port, servo_position); while (get_motor_position_counter(motor_port) < motor_position); off(motor_port); }
/* * Class: Motor * Method: clear_motor_position_counter * Signature: (I)I */ JNIEXPORT jint JNICALL Java_cbccore_low_Motor_clear_1motor_1position_1counter(JNIEnv* env, jobject obj, jint port) { #ifdef CBC return clear_motor_position_counter(port); #else printf("Java_cbccore_low_Motor_clear_1motor_1position_1counter stub\n"); return -1; #endif }
void lego_drive_until_opening(int direction, int speed) { int left_ticks_traveled = 0; int right_ticks_traveled = 0; float right_error; float left_error; float desired_value; int left_multiplier; int right_multiplier; int left_sensor_value; int right_sensor_value; clear_motor_position_counter(LEFT_MOTOR); clear_motor_position_counter(RIGHT_MOTOR); motor(LEFT_MOTOR, speed * direction); motor(RIGHT_MOTOR, speed * direction); while(TRUE) { left_sensor_value = analog_et(LEFT_ET); right_sensor_value = analog_et(RIGHT_ET); if (left_sensor_value < ET_NO_WALL && right_sensor_value < ET_NO_WALL) { break; } left_ticks_traveled = get_motor_position_counter(LEFT_MOTOR); right_ticks_traveled = get_motor_position_counter(RIGHT_MOTOR); desired_value = (left_ticks_traveled + right_ticks_traveled) / 2.0; left_error = desired_value - left_ticks_traveled; right_error = desired_value - right_ticks_traveled; left_multiplier = (int) ((left_error * l_kP) + 0.5); right_multiplier = (int) ((right_error * r_kP) + 0.5); motor(LEFT_MOTOR, (speed * direction) + left_multiplier); motor(RIGHT_MOTOR, (speed * direction) + right_multiplier); } freeze(LEFT_MOTOR); freeze(RIGHT_MOTOR); }
void LinkDrive(float distance, float speed) { mav(RIGHT_PORT, speed*LINK_RIGHT_OFFSET); mav(LEFT_PORT, speed*LINK_LEFT_OFFSET); clear_motor_position_counter(RIGHT_PORT); while(get_motor_position_counter(RIGHT_PORT) < distance*CM_TO_TICKS) { } ao(); }
void turn_right() { motor(0,0); motor(2,0); clear_motor_position_counter(0); mtp(0, 1000, 1050); block_motor_done(0); }
void turn_right() { motor(0,0); motor(2,0); clear_motor_position_counter(0); mtp(0, 1000, 850); // this is what got changed from 1050 to 950 to 850 block_motor_done(0); }
void turn_left() { motor(0,0); motor(2,0); clear_motor_position_counter(2); mtp(2, 1500, 1300); block_motor_done(2); }
void lego_spin_degrees(int degrees, int direction, int speed) { int left_ticks_traveled = 0; int right_ticks_traveled = 0; float right_error; float left_error; float desired_value; int left_multiplier; int right_multiplier; int ticks = (int) ((float) (degrees) * TICKS_PER_DEGREE); clear_motor_position_counter(LEFT_MOTOR); clear_motor_position_counter(RIGHT_MOTOR); motor(LEFT_MOTOR, speed * direction); motor(RIGHT_MOTOR, -speed * direction); while(TRUE) { if (abs(left_ticks_traveled) >= ticks || abs(right_ticks_traveled) >= ticks) { break; } left_ticks_traveled = abs(get_motor_position_counter(LEFT_MOTOR)); right_ticks_traveled = abs(get_motor_position_counter(RIGHT_MOTOR)); desired_value = (left_ticks_traveled + right_ticks_traveled) / 2.0; left_error = desired_value - left_ticks_traveled; right_error = desired_value - right_ticks_traveled; left_multiplier = (int) ((left_error * spin_l_kP) + 0.5); right_multiplier = (int) ((right_error * spin_r_kP) + 0.5); motor(LEFT_MOTOR, (speed * direction) + left_multiplier); motor(RIGHT_MOTOR, (-speed * direction) + right_multiplier); } freeze(LEFT_MOTOR); freeze(RIGHT_MOTOR); msleep(100); }
void moveFor(){ int frontDis = analog_et(FSS); int backDis = analog_et(BSS); int disDiff = frontDis - backDis; int turnLM = MOVE_FOR_TP - disDiff * KDp; int turnRM = MOVE_FOR_TP + disDiff * KDp; while(frontDis > 295){ motor(LM, turnLM); motor(RM, turnRM); frontDis = analog_et(FSS); backDis = analog_et(BSS); printf("%d\t%d\n", frontDis, backDis); disDiff = frontDis - backDis; turnLM = MOVE_BACK_TP - disDiff * KDp; turnRM = MOVE_BACK_TP + disDiff * KDp; } clear_motor_position_counter(LM); clear_motor_position_counter(RM); mtp(LM, 800, 500); mtp(RM, 800, 500); bmd(LM); bmd(RM); clear_motor_position_counter(LM); clear_motor_position_counter(RM); mtp(LM, 800, 260); mtp(RM, -800, -260); bmd(LM); bmd(RM); while(analog(RSS) < RSS_OFFSET){ motor(RM, 82); motor(LM, 90); } while(analog(RSS) > RSS_OFFSET){ motor(RM, 82); motor(LM, 90); } while(analog(RSS) < RSS_OFFSET){ motor(RM, 82); motor(LM, 90); } }
void lego_spin_until(int direction, int speed, int sensor, int (*comparator)(int, int), int threshold) { int left_ticks_traveled = 0; int right_ticks_traveled = 0; float right_error; float left_error; float desired_value; int left_multiplier; int right_multiplier; clear_motor_position_counter(LEFT_MOTOR); clear_motor_position_counter(RIGHT_MOTOR); motor(LEFT_MOTOR, speed * direction); motor(RIGHT_MOTOR, -speed * direction); while(TRUE) { if (comparator(analog(sensor), threshold)) { break; } display_printf(0, 0, "%4i", analog(sensor)); left_ticks_traveled = abs(get_motor_position_counter(LEFT_MOTOR)); right_ticks_traveled = abs(get_motor_position_counter(RIGHT_MOTOR)); desired_value = (left_ticks_traveled + right_ticks_traveled) / 2.0; left_error = desired_value - left_ticks_traveled; right_error = desired_value - right_ticks_traveled; left_multiplier = (int) ((left_error * spin_l_kP) + 0.5); right_multiplier = (int) ((right_error * spin_r_kP) + 0.5); motor(LEFT_MOTOR, (speed * direction) + left_multiplier); motor(RIGHT_MOTOR, (-speed * direction) + right_multiplier); } freeze(LEFT_MOTOR); freeze(RIGHT_MOTOR); }
void operate_winch(int position) { raise_winch(); clear_motor_position_counter(WINCH_MOTOR); if (position == WINCH_GROUND_POSITION) { motor(WINCH_MOTOR, 100); while (get_motor_position_counter(WINCH_MOTOR) < WINCH_GROUND_POSITION); freeze(WINCH_MOTOR); } else if (position == WINCH_MIDDLE_POSITION) { motor(WINCH_MOTOR, 100); while (get_motor_position_counter(WINCH_MOTOR) < WINCH_MIDDLE_POSITION); freeze(WINCH_MOTOR); } adjust_motor(WINCH_MOTOR, position); }
int main() { printf("Starting test of Blue and Green gripper.\n"); set_servo_position(BLUE_GRIPPER, BLUE_GRIPPER_START_POSITION); set_servo_position(GREEN_GRIPPER, GREEN_GRIPPER_START_POSITION); enable_servos(); press_A_to_continue(); set_servo_position(BLUE_GRIPPER, BLUE_GRIPPER_CLOSED_POSITION); press_A_to_continue(); set_servo_position(GREEN_GRIPPER, GREEN_GRIPPER_CLOSED_POSITION); press_A_to_continue(); printf("Starting test of spinner.\n"); clear_motor_position_counter(BLUE_SPINNER); motor(BLUE_SPINNER, 100); while (get_motor_position_counter(BLUE_SPINNER) < 225) ; off(BLUE_SPINNER); press_A_to_continue(); clear_motor_position_counter(BLUE_SPINNER); motor(BLUE_SPINNER, 100); while (get_motor_position_counter(BLUE_SPINNER) < 225) ; off(BLUE_SPINNER); press_A_to_continue(); clear_motor_position_counter(BLUE_SPINNER); motor(BLUE_SPINNER, 100); while (get_motor_position_counter(BLUE_SPINNER) < 225) ; off(BLUE_SPINNER); press_A_to_continue(); return 0; }
void main () { int loop_done = 0; int task_A_done = 0; int task_B_done = 0; int angle0 = 0; //get_create_total_ angle (.1); //create_drive_direct (left speed, right speed)\ //move_to_position (motor #, motor power(-for backwards), end position) //get_motor_done (motor#) //clear_motor_position_counter (motor#) //msleep (seconds) //i'll probably use this often create_connect (); clear_motor_position_counter (0); angle0 = get_create_total_angle (.1); create_drive_direct (150,-150); move_to_position (0, -900, -1550); while(! loop_done) { printf("a0: %d, a: %d, d: %d\n", angle0, get_create_total_angle (.1), task_A_done); if (angle0 - get_create_total_angle (.1) < 90) { create_drive_direct (150,-150); } else { task_A_done = 1; } if (get_motor_done (0)) { } else { task_B_done = 1; } if (task_A_done && task_B_done) { loop_done = 1; } msleep(0.1); } create_stop (); create_disconnect (); //put specific commands to control robot in here // loop_done = 0; // task_A_done = 0; // task_B_done = 0; // while(! loop_done) // { // if (/*task A is not done*/) // { // //continue doing task A // } // else // { // task_A_done = 1; // } // if (/*task B is not done*/) // { // //continue doing task B // } // else // { // task_B_done = 1; // } // if (task_A_done && task_B_done) // { // loop_done = 1; // } // } }
void lego_spin_onto_black_line(int direction, int speed, int low_threshold, int high_threshold) { int left_ticks_traveled = 0; int right_ticks_traveled = 0; float right_error; float left_error; float desired_value; int left_multiplier; int right_multiplier; int i; clear_motor_position_counter(LEFT_MOTOR); clear_motor_position_counter(RIGHT_MOTOR); motor(LEFT_MOTOR, speed * direction); motor(RIGHT_MOTOR, -speed * direction); for (i = 0; i < 3; i++) { while(TRUE) { if (analog_et(RIGHT_ET) >= low_threshold) { break; } left_ticks_traveled = abs(get_motor_position_counter(LEFT_MOTOR)); right_ticks_traveled = abs(get_motor_position_counter(RIGHT_MOTOR)); desired_value = (left_ticks_traveled + right_ticks_traveled) / 2.0; left_error = desired_value - left_ticks_traveled; right_error = desired_value - right_ticks_traveled; left_multiplier = (int) ((left_error * spin_l_kP) + 0.5); right_multiplier = (int) ((right_error * spin_r_kP) + 0.5); motor(LEFT_MOTOR, (speed * direction) + left_multiplier); motor(RIGHT_MOTOR, (-speed * direction) + right_multiplier); display_printf(0, 0, "L: %4i", ((speed * direction) + left_multiplier)); display_printf(0, 1, "R: %4i", ((speed * direction) + right_multiplier)); } msleep(10); while(TRUE) { if (analog_et(RIGHT_ET) <= high_threshold) { break; } left_ticks_traveled = abs(get_motor_position_counter(LEFT_MOTOR)); right_ticks_traveled = abs(get_motor_position_counter(RIGHT_MOTOR)); desired_value = (left_ticks_traveled + right_ticks_traveled) / 2.0; left_error = desired_value - left_ticks_traveled; right_error = desired_value - right_ticks_traveled; left_multiplier = (int) ((left_error * spin_l_kP) + 0.5); right_multiplier = (int) ((right_error * spin_r_kP) + 0.5); motor(LEFT_MOTOR, (-speed * direction) + left_multiplier); motor(RIGHT_MOTOR, (speed * direction) + right_multiplier); display_printf(0, 0, "L: %4i", ((speed * direction) + left_multiplier)); display_printf(0, 1, "R: %4i", ((speed * direction) + right_multiplier)); } } freeze(LEFT_MOTOR); freeze(RIGHT_MOTOR); }
void main (int argc, char ** argv) { int loop_done = 0; int task_A_done = 0; int task_B_done = 0; int angle = 0; int dangle = 0; int angle0 = 0; int distance0 = 0; int distance = 0 ; int ddistance = 0; // float lf_args[20]; // read inputs // printf("Args: %d\n", argc); // for (i = 2; i < argc; i++) // { // scanf("%lf%, argv[i] // } //get_create_total_ angle (.1); //create_drive_direct (left speed, right speed)\ //move_to_position (motor #, motor power(-for backwards), end position) //get_motor_done (motor#) //clear_motor_position_counter (motor#) //msleep (seconds) //i'll probably use this often create_connect (); clear_motor_position_counter (0); enable_servo (0); set_servo_position (0 ,1300); //set_servo_position (1800); // ------------------------------------------------------------------------ //step:1 aim the arm to knock over the box //raise arm printf("-----Step 1-----\n"); move_to_position (0, -900, -1200); while(! loop_done) { if (get_motor_done (0)) { loop_done = 1; } msleep(100); } create_stop (); loop_done = 0; angle0 = get_create_total_angle (.05); create_drive_direct (50,-50); while(! loop_done) { printf("a0: %d, a: %d, d: %d\n", angle0, get_create_total_angle (.05), task_A_done); if (angle0 - get_create_total_angle (.05) >= 20) { loop_done = 1; create_stop (); } msleep(100); } loop_done = 0; create_stop (); task_A_done = 0; task_B_done = 0; angle0 = get_create_total_angle (.05); create_drive_direct (-50,50); while(! loop_done) { printf("a0: %d, a: %d, d: %d\n", angle0, get_create_total_angle (.05), task_A_done); if (angle0 - get_create_total_angle (.05) <= -20) { loop_done = 1; create_stop (); } msleep(100); } loop_done = 0; create_stop (); task_A_done = 0; task_B_done = 0; // ------------------------------------------------------------------------ //step:2 knock over box and turn //lift arm //turn 90 printf("-----Step 2-----\n"); move_to_position (0, -900, -3100); msleep (900); angle0 = get_create_total_angle (.05); angle = angle0; dangle = abs (angle0 - angle); printf("a0: %d, a: %d, da: %d, d: %d\n", angle0, get_create_total_angle (.05), dangle , task_A_done); create_drive_direct (-178,-648); while(! loop_done) { if (dangle >= 125) { create_drive_direct (-120, -181); } if (dangle >= 155) { task_A_done = 1; create_stop (); } if (get_motor_done (0)) { task_B_done = 1; } if (task_A_done && task_B_done) { loop_done = 1; } msleep(100); angle = get_create_total_angle (.05); dangle = abs (angle0 - angle); printf("a0: %d, a: %d, da: %d, d: %d\n", angle0, get_create_total_angle (.05), dangle , task_A_done); } loop_done = 0; create_stop (); task_A_done = 0; task_B_done = 0; // ------------------------------------------------------------------------ //step:4 aim claw to point of botguy //turn 20 //lower arm printf("-----Step 4-----\n"); angle0 = 0; angle0 = get_create_total_angle (.05); create_drive_direct (70, -70); printf("a0: %d, a: %d\n", angle0, get_create_total_angle(0.05)); //while(angle0 - get_create_total_angle (.05) < 4) if (0) { printf("a0: %d, a: %d\n", angle0, get_create_total_angle(0.05)); msleep(50); } move_to_position (0, 400, -900); distance0 = get_create_distance (.05); create_drive_direct (20, 20); while (! loop_done) { if (get_create_distance (.05) - distance0 > 50) { loop_done = 1; create_stop (); } } set_servo_position (0 ,400); loop_done = 0; task_A_done = 0; task_B_done = 0; create_drive_straight (-20); move_to_position (0, 600, -300); while (! get_motor_done (0)) { msleep (50); } create_drive_straight (-50); msleep (2000); //while (! loop_done) if (0) { if (digital (10)) { create_stop (); loop_done = 1; } if (get_create_distance (.05) - distance0 < -100) { create_stop (); loop_done = 1; } } loop_done = 0; // ------------------------------------------------------------------------ //step:5 grab botguy and lift him up (not complete) printf("-----Step 5-----\n"); create_drive_direct (60, -60); while(angle0 - get_create_total_angle (.05) < 3) { msleep(50); } create_stop (); set_servo_position (0, 1510); move_to_position (0, -1000, -3000); msleep (5000); // ------------------------------------------------------------------------ //step:6 move backwards untill the bumper hits the transport (not completed) printf("-----Step 6-----\n"); create_drive_direct (130, 280); distance0 = get_create_distance (.05); while (! loop_done) { printf("d0: %d, d: %d, d: %d\n", distance0, get_create_distance (.05), loop_done); if (get_create_distance (.05) - distance0 > 700) { loop_done = 1; } msleep(50); } loop_done = 0; move_to_position (0, -900, -500); create_drive_straight (-100); msleep (1000); set_servo_position (0, 200); create_disconnect (); printf("-----Step 6-B-----\n"); create_drive_direct (-400, -150); distance0 = get_create_distance (.05); while(! loop_done) { printf("d0: %d, d: %d, d: %d\n", distance0, get_create_distance (.05), loop_done); if (get_create_distance (.05) - distance0 < -100) { loop_done = 1; create_stop (); } msleep(50); } loop_done = 0; // ------------------------------------------------------------------------ //step:7 drop botguy. printf("-----Step 7-----\n"); move_to_position (0, -900, -300); msleep (6000); set_servo_position (0, 200); create_disconnect (); }
void MoveForwardMillimeters(int mmToGo, int Speed) { //Calculate the number of ticks to go float RightTicksToGo = TICKS_PER_REVOLUTION_RIGHT*(mmToGo/MILLIMETERS_PER_REVOLUTION); float LeftTicksToGo = TICKS_PER_REVOLUTION_LEFT*(mmToGo/MILLIMETERS_PER_REVOLUTION); //Calculate mm per tick float RightTicksPerMM = TICKS_PER_REVOLUTION_RIGHT/MILLIMETERS_PER_REVOLUTION; float LeftTicksPerMM = TICKS_PER_REVOLUTION_LEFT/MILLIMETERS_PER_REVOLUTION; //Set Motor Ticks position to zero clear_motor_position_counter(LEFT_PORT_WHEEL); clear_motor_position_counter(RIGHT_PORT_WHEEL); //Set Motor Positions to a variable .. should be zero here because we just cleared them int LeftMotorStartPosition = get_motor_position_counter(LEFT_PORT_WHEEL); int RightMotorStartPosition = get_motor_position_counter(RIGHT_PORT_WHEEL); //Calculate Motor Final Tick Position for MTP Command int LeftMotorFinalPosition = LeftMotorStartPosition + LeftTicksToGo; int RightMotorFinalPosition = RightMotorStartPosition + RightTicksToGo; //Get time of start movement //Not used yet.. for timeout functionality double StartMovementTime = seconds(); //Loop until movment complete as measured by the left wheel while(get_motor_position_counter(LEFT_PORT_WHEEL) < LeftMotorFinalPosition) { //Calculate Wheel Distance Moved each loop int LeftDistanceMoved = (get_motor_position_counter(LEFT_PORT_WHEEL)-LeftMotorStartPosition)/LeftTicksPerMM; int RightDistanceMoved = (get_motor_position_counter(RIGHT_PORT_WHEEL)-RightMotorStartPosition)/RightTicksPerMM; //Calculate Wheel Split //Positive Values Right Wheel is Ahead, Negative Values Left Wheel is ahead int WheelSplit = RightDistanceMoved - LeftDistanceMoved; //See if Wheel Split is exceeding the Correction Threshhold in either direction if(WheelSplit < CORRECTION_THRESHOLD && WheelSplit > -CORRECTION_THRESHOLD) { //Wheels are going straight motor(RIGHT_PORT_WHEEL, 100); motor(LEFT_PORT_WHEEL, 100); //move_to_position(RIGHT_PORT_WHEEL, Speed, RightMotorFinalPosition); //move_to_position(LEFT_PORT_WHEEL, Speed, LeftMotorFinalPosition); printf("STRAIGHT: left: %d right: %d split: %d\n", LeftDistanceMoved, RightDistanceMoved, WheelSplit); } else if(WheelSplit >= CORRECTION_THRESHOLD) { //Right wheel is ahead of left wheel //correction reduction applied to right wheel motor(RIGHT_PORT_WHEEL, 100*CORRECTION_REDUCTION); motor(LEFT_PORT_WHEEL, 100); //move_to_position(RIGHT_PORT_WHEEL, Speed * CORRECTION_REDUCTION, RightMotorFinalPosition); //move_to_position(LEFT_PORT_WHEEL, Speed, LeftMotorFinalPosition); printf("CORRECT RIGHT\n"); } else if(WheelSplit <= -CORRECTION_THRESHOLD) { //Left wheel is ahead of right wheel //correction reduction applied to left wheel motor(RIGHT_PORT_WHEEL, 100); motor(LEFT_PORT_WHEEL, 100*CORRECTION_REDUCTION); //move_to_position(RIGHT_PORT_WHEEL, Speed, RightMotorFinalPosition); //move_to_position(LEFT_PORT_WHEEL, Speed * CORRECTION_REDUCTION, LeftMotorFinalPosition); printf("CORRECT LEFT\n"); } msleep(150); } //Turn All Motors Off ao(); }
int main() { /* FIRST CALIBRATION: based on poms position adjust starting position */ printf("test 3.11, calibration value: \nclaw down : %d, \nright ang forward right: %d, \nright ang forward left : %d, \nback right angle %d ...\n", DOWN_SERVO, RIGHT_ANGLE_CLICKS_MRP_RIGHT, RIGHT_ANGLE_CLICKS_MRP_LEFT, RIGHT_ANGLE_CLICKS_BACK_MRP); double start_time = seconds(); enable_servos(); clear_motor_position_counter(RIGHT_MOTOR); clear_motor_position_counter(LEFT_MOTOR); wait_for_light(1); shut_down_in(118); clawUp(); //************************************** //* PART 1 : get first set of poms to lower storage area //* //************************************** moveBackward(1, NO_DEBUG); moveForwardHighSpeed(23, DEBUG); clawDown(); //moveForward(8,NO_DEBUG); //we have the Poms rightAngleBwd(RIGHT, NO_DEBUG); //bump against the upper storage area moveBackwardHighSpeed(15, NO_DEBUG); moveForwardHighSpeed(17, NO_DEBUG); rightAngleBwd(RIGHT, NO_DEBUG); //bump against upper PVC side moveBackwardHighSpeed(18, NO_DEBUG); //move towards dropping poms moveForward(1.5, NO_DEBUG); msleep(300); rightAngleFwd(LEFT, NO_DEBUG); msleep(300); clawUp(); //uses IR sensor to stop the forward movement moveForwardTilBlackLine(15, NO_DEBUG); printf("====> elapsed time end of part 1: %f\n", (seconds() - start_time)); //************************************** //* END OF PART 1 : get first set of poms to lower storage area //* //************************************** //************************************** //* PART 2 : get the blue cube to the corner //* //************************************** moveBackwardHighSpeed(25, NO_DEBUG); //recalibrate against top PVC next to botguy rightAngleBwd(LEFT, NO_DEBUG); moveBackwardHighSpeed(11, NO_DEBUG); // SECOND CALIBRATION: based on blackline/blue position moveForward(7.5, NO_DEBUG); rightAngleFwd(RIGHT, NO_DEBUG); //get the cube now //====>>>>> may have to be adjust the day of competition based on position of 2nd //====>>>>> set of poms moveForwardHighSpeed(15, NO_DEBUG); //====>>>>> clawAlmostDownCube(); twentyTwoAngleFwd(LEFT, NO_DEBUG); clawUp(); //====>>>>> calibration to get to the corner 22.5 fwd, 22.5 more forward, aiming at bottom left hand corner moveForwardHighSpeed(2.5, NO_DEBUG); twentyTwoAngleFwd(LEFT, NO_DEBUG); //clawAlmostDownCube(); moveForwardHighSpeed(30, NO_DEBUG); //====>>>>> //moveForwardHighSpeed(2, NO_DEBUG); //====>>>>> moveBackwardHighSpeed(9, NO_DEBUG); fortyFiveAngleFwd(LEFT, NO_DEBUG); printf("====> elapsed time end of part 2: %f\n", (seconds() - start_time)); //************************************** //* END OF PART 2 : get the blue cube in the corner //* //************************************** //************************************** //* PART 3 : prepare to get second set of poms //* //************************************** //recalibrate against side PVC rightAngleBwd(RIGHT, NO_DEBUG); moveBackwardHighSpeed(25, NO_DEBUG); moveForward(5, NO_DEBUG); //calibrating with top PVC rightAngleBwd(LEFT,NO_DEBUG); moveBackwardHighSpeed(23,NO_DEBUG); // THIRD CALIBRATION: based on second set of Poms //below is the number of inches from the side PVC // moveForwardHighSpeed(10, NO_DEBUG); rightAngleBwd(RIGHT, NO_DEBUG); //recalibrate with side pvc moveBackwardHighSpeed(5,NO_DEBUG); printf("====> elapsed time end of part 3: %f\n", (seconds() - start_time)); //************************************** //* END OF PART 3 : prepare to get second set of poms //* //************************************** //************************************** //* PART 4 : bring second set of poms home //* //************************************** //grabs the poms, bring down claw and go home moveForwardHighSpeed(12.5,NO_DEBUG); clawDown(); moveForwardHighSpeed(59.5, NO_DEBUG); //here is the code to recalibrate one more time... but do we have enough time? /*//recalibrate against top PVC rightAngleBwd(LEFT,NO_DEBUG); moveBackwardHighSpeed(10, NO_DEBUG); //move towards dropping poms moveForwardHighSpeed(10, NO_DEBUG); //msleep(500); printf("==> moving righ angle bwd\n"); rightAngleFwd(LEFT, NO_DEBUG); */ //recalibrate against top PVC //moveForwardHighSpeed(41,NO_DEBUG); //last calibration before dropping second set of poms rightAngleBwd(LEFT,NO_DEBUG); moveBackwardHighSpeed(14, NO_DEBUG); //move towards dropping poms moveForwardSlow(1.5, NO_DEBUG); msleep(300); rightAngleFwd(LEFT, NO_DEBUG); msleep(300); clawUp(); moveForwardTilBlackLine(15, DEBUG); //************************************** //* END OF PART 4 : bring second set of poms home //* //************************************** printf("====> elapsed time: %f\n", (seconds() - start_time)); ao(); disable_servos(); return 0; }
void CombinedMotorWidget::clearPosition() { clear_motor_position_counter(ui->motors->currentIndex()); }