/* Moves the robot forward distances inches at speed */ void drive_forward(int distance, int speed) { int right, left, clicks; float angle_target; float tolerance; float angle = (theta / lsb_ms_per_deg); tolerance = lsb_ms_per_deg * 0.5; /* +/-0.5 degree accuracy */ angle_target = angle * lsb_ms_per_deg; clicks = to_clicks(distance); enable_encoder(LEFT_ENCODER); enable_encoder(RIGHT_ENCODER); while (clicks > right) { left = read_encoder(LEFT_ENCODER); right = read_encoder(RIGHT_ENCODER); if (theta < (angle_target - tolerance)) { /* Turn CCW a little */ motor(LEFT_MOTOR, speed - TWEAK); motor(RIGHT_MOTOR, speed + TWEAK); } else if (theta > (angle_target + tolerance)){ /* Turn CW a little */ motor(LEFT_MOTOR, speed + TWEAK); motor(RIGHT_MOTOR, speed - TWEAK); } else { motor(LEFT_MOTOR, speed); motor(RIGHT_MOTOR, speed); } } disable_encoder(LEFT_ENCODER); disable_encoder(RIGHT_ENCODER); moff(); } void drive_backward(float angle, int speed) { float angle_target; float tolerance; tolerance = lsb_ms_per_deg * 0.5; /* +/-0.5 degree accuracy */ angle_target = angle * lsb_ms_per_deg; if (theta > (angle_target + tolerance)) { /* Turn CCW a little */ motor(LEFT_MOTOR, speed - TWEAK); motor(RIGHT_MOTOR, speed + TWEAK); } else if (theta < (angle_target - tolerance)){ /* Turn CW a little */ motor(LEFT_MOTOR, speed + TWEAK); motor(RIGHT_MOTOR, speed - TWEAK); } else { motor(LEFT_MOTOR, speed); motor(RIGHT_MOTOR, speed); } } void move_forward(float angle, int speed) { float angle_target; float tolerance; tolerance = lsb_ms_per_deg * 0.5; /* +/-0.5 degree accuracy */ angle_target = angle * lsb_ms_per_deg; if (theta < (angle_target - tolerance)) { /* Turn CCW a little */ motor(LEFT_MOTOR, speed - TWEAK); motor(RIGHT_MOTOR, speed + TWEAK); } else if (theta > (angle_target + tolerance)){ /* Turn CW a little */ motor(LEFT_MOTOR, speed + TWEAK); motor(RIGHT_MOTOR, speed - TWEAK); } else { motor(LEFT_MOTOR, speed); motor(RIGHT_MOTOR, speed); } } /* Turns to robot to the specified angle To convert degrees to units of theta, multiply by lsb_ms_per_deg. Demo robot turns at about 120 deg/s when speed = 100 */ void go_to_angle(float angle) { float angle_target; float error, tolerance; int speed; tolerance = 1.0 * lsb_ms_per_deg; angle_target = angle * lsb_ms_per_deg; error = theta - angle_target; if ( fabs(error) > tolerance ) { spin(40 * -sign(error)); while(fabs(error) > (80.0 * lsb_ms_per_deg)) { error = theta - angle_target; } spin(30 * -sign(error)); while(fabs(error) > (60.0 * lsb_ms_per_deg)) { error = theta - angle_target; } spin(20 * -sign(error)); while(fabs(error) > (10.0 * lsb_ms_per_deg)){ error = theta - angle_target; } /* Slam on the brakes */ spin(100 * sign(error)); msleep(10L); /* Fine adjust either direction as necessary */ spin(10 * -sign(error)); while(fabs(error) > tolerance){ error = (theta - angle_target); spin(10 * -sign(error)); } spin(0); } } /* Turns the robot 90 degrees to the right */ void turn_right() { go_to_angle((theta / lsb_ms_per_deg) - 90.0); } /* Turns the robot 90 degrees to the left */ void turn_left() { go_to_angle((theta / lsb_ms_per_deg) + 90.0); } /* Turns the robot 90 degrees to the right (wide) */ void turn_wide_right() { float angle = (theta / lsb_ms_per_deg) - 90.0; float angle_target; float error, tolerance; int speed; long time; tolerance = 1.0 * lsb_ms_per_deg; angle_target = angle * lsb_ms_per_deg; error = theta - angle_target; moff(); if ( fabs(error) > tolerance ) { motor(LEFT_MOTOR,40); time = mseconds(); while(fabs(error) > (80.0 * lsb_ms_per_deg)) { error = theta - angle_target; if ((mseconds() - time) > 2000L) motor(RIGHT_MOTOR,-25); } motor(LEFT_MOTOR,30); time = mseconds(); while(fabs(error) > (60.0 * lsb_ms_per_deg)) { error = theta - angle_target; if ((mseconds() - time) > 2000L) motor(RIGHT_MOTOR,-25); } motor(LEFT_MOTOR,20); time = mseconds(); while(fabs(error) > (10.0 * lsb_ms_per_deg)) { error = theta - angle_target; if ((mseconds() - time) > 2000L) motor(RIGHT_MOTOR,-25); } /* Slam on the brakes */ off(RIGHT_MOTOR); off(LEFT_MOTOR); msleep(10L); /* Fine adjust either direction as necessary */ motor(LEFT_MOTOR,10); while(fabs(error) > tolerance){ error = (theta - angle_target); motor(LEFT_MOTOR,10); } moff(); } } /* Turns the robot 90 degrees to the left (wide) */ void turn_wide_left() { float angle = (theta / lsb_ms_per_deg) + 90.0; float angle_target; float error, tolerance; int speed; long time; tolerance = 1.0 * lsb_ms_per_deg; angle_target = angle * lsb_ms_per_deg; error = theta - angle_target; moff(); if ( fabs(error) > tolerance ) { motor(RIGHT_MOTOR,40); time = mseconds(); while(fabs(error) > (80.0 * lsb_ms_per_deg)) { error = theta - angle_target; if ((mseconds() - time) > 2000L) motor(LEFT_MOTOR,-25); } motor(RIGHT_MOTOR,30); time = mseconds(); while(fabs(error) > (60.0 * lsb_ms_per_deg)) { error = theta - angle_target; if ((mseconds() - time) > 2000L) motor(LEFT_MOTOR,-25); } motor(RIGHT_MOTOR,20); time = mseconds(); while(fabs(error) > (10.0 * lsb_ms_per_deg)) { error = theta - angle_target; if ((mseconds() - time) > 2000L) motor(LEFT_MOTOR,-25); } /* Slam on the brakes */ off(RIGHT_MOTOR); off(LEFT_MOTOR); msleep(10L); /* Fine adjust either direction as necessary */ motor(RIGHT_MOTOR,10); while(fabs(error) > tolerance){ error = (theta - angle_target); motor(RIGHT_MOTOR,10); } moff(); } } /* Turns the robot 180 degrees */ void turn_around() { float angle = theta; go_to_angle((angle / lsb_ms_per_deg) + 180.0); } void spin(int speed) { motor(LEFT_MOTOR, -speed); motor(RIGHT_MOTOR, speed); } int sign(float a) { if ( a > 0.0 ) return 1; else return -1; } float fabs(float a) { if ( a > 0.0 ) return a; else return -a; } void moff() { off(LEFT_MOTOR); off(RIGHT_MOTOR); } int to_clicks(int distance) { return ((int)(52.543689320388349 * (float)distance)); } void display_clicks(){ for(;;){ printf("RIGHTL %d LEFT %d\n", read_encoder(RIGHT_ENCODER), read_encoder(LEFT_ENCODER)); msleep(200L); defer(); } }