/* * The MIT License * * Copyright (c) 2007 MIT 6.270 Robotics Competition * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. * */ /* LSB_US_PER_DEG for gyro instead of LSB_MS_PER_DEG (new joyos version) */ // Include headers from OS #include #include #include // necessary? #include "Build_0.h" #define RIGHT_MOTOR 0 #define LEFT_MOTOR 1 #define CLAWS_MOTOR 2 #define EXTRA_MOTOR 3 // unused as of now #define RIGHT_SENCODER 24 #define LEFT_SENCODER 26 // keep these in order for crappily written functions... #define FRONT_DIST_SENS 20 #define REAR_DIST_SENS 21 #define LEFT_DIST_SENS 22 #define RIGHT_DIST_SENS 23 #define DIST_SENS_ORIENT_THRESHOLD 350 #define NUM_DRIVE_MOTORS 2 #define NUM_MOTORS 3 #define NUM_DIST_SENS 4 // front, rear, left, right (doesn't include LIDAR) #define TEST_BUMP_SENSOR 0 #define TEST_MOTOR_SPEED 100 #define DEFAULT_PAUSE 100 #define SHORT_PAUSE 50 #define GOAL_MOTOR_SPEED 100 #define GOAL_MOTOR_SPEED_RIGHT 175//80 #define GOAL_MOTOR_SPEED_LEFT 140//87 #define MAJOR_SPEED_ADJUST_INCREMENT 6 #define MINOR_SPEED_ADJUST_INCREMENT 3 #define MOTOR_SPEED_TURNING_RIGHT_HIGH 135 #define MOTOR_SPEED_TURNING_LEFT_HIGH 130 #define MOTOR_SPEED_TURNING_RIGHT_LOW 70 #define MOTOR_SPEED_TURNING_LEFT_LOW 70 #define ENCODING_SCALAR 4.5 // 4.5cm for each tick of the shaft encoder //wrong #define THREE_FEET 15 // ceil(3 * 2.54 * ENCODING_SCALAR) #define RIGHT90 -90 #define LEFT90 90 #define SMALL_DIST 2 // maybe don't need? #define ONE_FOOT 5 #define TWO_FEET 12 // asmt. 4 #define SIX_FEET 32 // asmt. 4 #define FIVE_FEET 27 // asmt. 4 #define DEFAULT_REVERSE 200 // ms for reversing #define MOTOR_SPEED_REVERSING_RIGHT -100 //-50 #define MOTOR_SPEED_REVERSING_LEFT -100 //-55 #define MOTOR_CURRENT_SPIKE_THRESHOLD 1000 // in mA // valid for forward and reverse or need to separate? #define MOTOR_SPIKE_ERROR 200 #define MOTOR_STOP_PAUSE 300 #define DRIVE_BRAKE 1000 #define DEFAULT_TIMEOUT 2000 #define GYRO_PORT 11 #define LSB_US_PER_DEG 1400000 #define MOTOR_SPEED_CLAWS_OPEN 255 #define MOTOR_SPEED_CLAWS_CLOSE -255 #define CLAWS_MOTOR_THRESHOLD 2000 #define CLAWS_TIMEOUT 700 #define CLAWS_GOAL_TIMEOUT 50 #define FORWARD_STATE 0 #define REVERSE_STATE 1 #define TURN_STATE 2 #define MOTOR_SPIKE_STATE 3 #define ADJUST_LEFT 4 // depreciated #define ADJUST_RIGHT 5 // depreciated #define NEEDS_ORIENTATION 6 // entirely unwritten #define DEBUG_STATE 1000 #define STARTING_POSITION 0 #define GOAL_POSITION 1 #define FLAG_POSITION 2 #define FIELD_POSITION 3 #define CLAWS_CLOSED 0 #define CLAWS_OPEN 1 #define CLAWS_GOAL 2 #define CLAWS_RECLOSE 3 #define AT_WALL 0 #define VERY_CLOSE 1 #define CLOSE 2 #define NEAR 3 #define MODERATE 4 #define MOD_FAR 5 #define FAR 6 #define QUITE_FAR 7 #define VERY_FAR 8 #define INDETERMINATE 50 uint8_t front_close_to_wall, rear_close_to_wall; int claws_state, claws_position; uint8_t claws_thread_on; int drive_state; // one of FORWARD_STATE, ADJUST_LEFT, ADJUST_RIGHT, REVERSE_STATE, now others... uint8_t drive_thread_on; float drive_starting_angle; int16_t right_motor_speed; int16_t left_motor_speed; uint8_t motor_spike_thread_on; //required for new joyos version uint8_t team_number[2] = {5,0}; uint8_t orientation_type; // STARTING_POSITION, GOAL_POSITION, FLAG_POSITION, FIELD_POSITION void assignment4() { //orient_robot(2); orient_robot(NUM_DIST_SENS); reverse_until_motor_spike(2000); // added //claws_state = CLAWS_OPEN; //drive_straight(TWO_FEET, CLAWS_CLOSED); drive_straight(ONE_FOOT, CLAWS_CLOSED); spin_using_gyro(RIGHT90, DEFAULT_TIMEOUT); reverse_until_motor_spike(2000); claws_state = CLAWS_OPEN; open_claws(); //motor_spike_thread_on = 1; drive_straight(FIVE_FEET, CLAWS_CLOSED); close_claws(); /* motor_spike_thread_on = 1; drive_straight(8, CLAWS_CLOSED); while(drive_thread_on) { pause(10); } motor_spike_thread_on = 0; //claws_state = CLAWS_CLOSED; // eh? spin_using_gyro(RIGHT90, DEFAULT_TIMEOUT); */ spin_using_gyro(-40.0, DEFAULT_TIMEOUT); claws_state = CLAWS_OPEN; // to fix above open_claws(); pause(50); drive_straight(3, CLAWS_OPEN); } uint8_t read_dist_sens_range(uint8_t sensor) { return dist_sens_scale(sensor); } uint8_t dist_sens_scale(uint8_t sensor) { switch(sensor) { case FRONT_DIST_SENS: return front_dist_sens_scale(analog_read(FRONT_DIST_SENS)); break; case REAR_DIST_SENS: return rear_dist_sens_scale(analog_read(REAR_DIST_SENS)); break; case LEFT_DIST_SENS: // add later return side_dist_sens_scale(analog_read(LEFT_DIST_SENS)); break; case RIGHT_DIST_SENS: // add later return side_dist_sens_scale(analog_read(RIGHT_DIST_SENS)); break; default: break; } return INDETERMINATE; } uint8_t front_dist_sens_scale(uint16_t analog) { uint8_t front_dist_to_wall = INDETERMINATE; if(front_close_to_wall) { if(analog < 400) front_dist_to_wall = AT_WALL; else front_dist_to_wall = VERY_CLOSE; } else { if(analog > 500) { front_dist_to_wall = CLOSE; } else if(analog > 400) { front_dist_to_wall = NEAR; } else if(analog > 300) { front_dist_to_wall = MODERATE; } else if(analog > 225) { front_dist_to_wall = MOD_FAR; } else if(analog > 170) { front_dist_to_wall = FAR; } else if(analog > 100) { front_dist_to_wall = QUITE_FAR; } else // analog <= 100 { front_dist_to_wall = VERY_FAR; } } return front_dist_to_wall; } uint8_t rear_dist_sens_scale(uint16_t analog) { uint8_t rear_dist_to_wall = INDETERMINATE; if(rear_close_to_wall) { if(analog < 400) rear_dist_to_wall = AT_WALL; else rear_dist_to_wall = VERY_CLOSE; } else { if(analog > 500) { rear_dist_to_wall = CLOSE; } else if(analog > 400) { rear_dist_to_wall = NEAR; } else if(analog > 300) { rear_dist_to_wall = MODERATE; } else if(analog > 225) { rear_dist_to_wall = MOD_FAR; } else if(analog > 170) { rear_dist_to_wall = FAR; } else if(analog > 100) { rear_dist_to_wall = QUITE_FAR; } else // analog <= 100 { rear_dist_to_wall = VERY_FAR; } } return rear_dist_to_wall; } uint8_t side_dist_sens_scale(uint16_t analog) { uint8_t dist_to_wall = INDETERMINATE; if(analog > 500) { dist_to_wall = CLOSE; } else if(analog > 400) { dist_to_wall = NEAR; } else if(analog > 300) { dist_to_wall = MODERATE; } else if(analog > 225) { dist_to_wall = MOD_FAR; } else if(analog > 170) { dist_to_wall = FAR; } else if(analog > 100) { dist_to_wall = QUITE_FAR; } else // analog <= 100 { dist_to_wall = VERY_FAR; } return dist_to_wall; } uint8_t orient_robot(uint8_t num_sensors) { //uint8_t orientation_code; // debug? maybe omit later? switch(orientation_type) { case STARTING_POSITION: return orient_at_start(num_sensors); break; case GOAL_POSITION: break; case FLAG_POSITION: break; case FIELD_POSITION: break; default: break; } return 0; } // orients robot from starting position to have walls on left and rear // returns 0 if operates correctly; returns 1 if an error occurs during sensing; returns 2 if num_sensors is bad (i.e., not 2 or 4) // // with 4 sensors, robot reads from all 4, knows its orientation, and rotates once // with 2 sensors, robot reads from front and back, rotates 90 degrees, reads front and back again, and then makes any necessary corrections uint8_t orient_at_start(uint8_t num_sensors) { if(num_sensors == 2) { switch(read_dist_sens_digital(2)) { case 1: // front activated spin_using_gyro(RIGHT90, DEFAULT_TIMEOUT); switch(read_dist_sens_digital(2)) { case 1: // front activated spin_using_gyro(RIGHT90, DEFAULT_TIMEOUT); break; case 2: // back activated // do nothing break; default: //error return 1; break; } break; case 2: // back activated spin_using_gyro(LEFT90, DEFAULT_TIMEOUT); switch(read_dist_sens_digital(2)) { case 1: // front activated spin_using_gyro(RIGHT90, DEFAULT_TIMEOUT); break; case 2: // back activated // do nothing break; default: // error return 1; break; } break; default: // error return 1; break; } } else if(num_sensors == 4) { switch(read_dist_sens_digital(4)) { case 5: spin_using_gyro(RIGHT90, DEFAULT_TIMEOUT); return 5; // debug break; case 6: // do nothing return 6; // debug break; case 9: spin_using_gyro(-180, DEFAULT_TIMEOUT); return 9; // debug break; case 10: spin_using_gyro(LEFT90, DEFAULT_TIMEOUT); return 10; // debug break; default: // error return 1; break; } } else // bad { return 2; } return 0; } uint8_t reverse_until_motor_spike(uint16_t timeout) { uint8_t old_drive_state = drive_state; uint16_t final_time = get_time() + timeout; drive_state = REVERSE_STATE; motor_spike_thread_on = 0; drive_thread_on = 1; start_reverse_motors(); while( (!detect_motor_current_spikes()) && (final_time > get_time())) { pause(50); } drive_thread_on = 0; stop_drive_motors(); return old_drive_state; } // resets value of gyroscope, so can't use in another procedure that requires continuous gyro readings //nevermind // assumes robot starts from rest // degrees work sugh that left (counterclockwise) is positive (unless changed later) uint8_t spin_using_gyro(float turn_degrees, uint32_t max_time_ms) { float goal_degrees, delta_degrees; //uint32_t max_time_left, final_time; uint32_t final_time; //max_time_left = max_time_ms; drive_state = TURN_STATE; //gyro_set_degrees(0); goal_degrees = gyro_get_degrees() + turn_degrees; //might need a - instead of a + delta_degrees = turn_degrees; //might need to be negated as well... final_time = get_time() + max_time_ms; while((fabs(delta_degrees) > 1.6) && (final_time > get_time())) // check boolean stuff here { if(delta_degrees > 15) { // pivot left if(left_motor_speed != -MOTOR_SPEED_TURNING_LEFT_HIGH) { set_motor_speed(LEFT_MOTOR, -MOTOR_SPEED_TURNING_LEFT_HIGH); set_motor_speed(RIGHT_MOTOR, MOTOR_SPEED_TURNING_RIGHT_HIGH); } } else if(delta_degrees < -15) { // pivot right if(left_motor_speed != MOTOR_SPEED_TURNING_LEFT_HIGH) { set_motor_speed(LEFT_MOTOR, MOTOR_SPEED_TURNING_LEFT_HIGH); set_motor_speed(RIGHT_MOTOR, -MOTOR_SPEED_TURNING_RIGHT_HIGH); } } else if(delta_degrees > 0) { // slow pivot left if(left_motor_speed != -MOTOR_SPEED_TURNING_LEFT_LOW) { set_motor_speed(LEFT_MOTOR, -MOTOR_SPEED_TURNING_LEFT_LOW); set_motor_speed(RIGHT_MOTOR, MOTOR_SPEED_TURNING_RIGHT_LOW); } } else { // slow pivot right if(left_motor_speed != MOTOR_SPEED_TURNING_LEFT_LOW) { set_motor_speed(LEFT_MOTOR, MOTOR_SPEED_TURNING_LEFT_LOW); set_motor_speed(RIGHT_MOTOR, -MOTOR_SPEED_TURNING_RIGHT_LOW); } } pause(10); //max_time_left = final_time - get_time(); delta_degrees = goal_degrees - gyro_get_degrees(); // check this; maybe reverse to make real delta later... //printf_P (PSTR("\nAngle left: %.2f"), delta_degrees);; // debug; get rid of this asap } pause(20); //coast_drive_motors(); stop_drive_motors(); /* if(max_time_left <= 0) { printf("\nTime kill"); pause(500); } else { printf("\nTurn Complete (now coasting)"); pause(500); } */ //pause(100); // eh? return 0; } // tests both drive motors for current spikes over a short time period // returns: 0 - if neither motor is spiking // 1 - if left motor is spiking // 2 - if right motor is spiking // 3 - if both motors are spiking uint8_t detect_motor_current_spikes() { uint8_t countr, countl; countr = 0; countl = 0; for(int i = 0; i <= 10; i++) { if(motor_get_current_MA(RIGHT_MOTOR) >= MOTOR_CURRENT_SPIKE_THRESHOLD) countr++; if(motor_get_current_MA(LEFT_MOTOR) >= MOTOR_CURRENT_SPIKE_THRESHOLD) countl++; pause(5); } if(countr > 5) countr = 2; else countr = 0; if(countl > 5) countl = 1; else countl = 0; return (countr + countl); } // altered afternoon 28-1-09; may need more alteration once field orientation code is written int motor_current_spike_thread() { uint8_t current_spikes, old_drive_state; while(1) { while(motor_spike_thread_on) //&& (drive_state == FORWARD_STATE)) // drive_state necessary??? { current_spikes = detect_motor_current_spikes(); if(current_spikes) { old_drive_state = drive_state; // added if(drive_state == FORWARD_STATE) { drive_state = MOTOR_SPIKE_STATE; //printf("\nRight: %d Left: %d", motor_get_current_MA(RIGHT_MOTOR), motor_get_current_MA(LEFT_MOTOR)); // debug //printf("\nSpike: %d, %d, %d", current_spikes, drive_thread_on, drive_state); //debug stop_drive_motors(); pause(MOTOR_STOP_PAUSE); drive_reverse(SMALL_DIST); drive_thread_on = 0; } // end FORWARD_STATE else if(drive_state == REVERSE_STATE) { drive_state = MOTOR_SPIKE_STATE; //printf("\nRight: %d Left: %d", motor_get_current_MA(RIGHT_MOTOR), motor_get_current_MA(LEFT_MOTOR)); // debug stop_drive_motors(); pause(MOTOR_STOP_PAUSE); drive_thread_on = 0; } // end REVERSE_STATE } else pause(10); } yield(); //pause(200); // eh? } return 0; } // usetup is called during the calibration period. It must return before the // period ends. int usetup (void) { //set_auto_halt(0); // meh printf("\nCalibrating"); pause(500); gyro_init(GYRO_PORT, LSB_US_PER_DEG, 1000L); pause(500); printf("\nStarting"); drive_starting_angle = gyro_get_degrees(); // unnecessary orientation_type = STARTING_POSITION; create_thread(&motor_current_spike_thread, STACK_DEFAULT, 0, "motor_spike"); motor_spike_thread_on = 0; // correct initialization? create_thread(&drive_straight_thread, STACK_DEFAULT, 1, "drive_straight"); drive_thread_on = 0; drive_state = FORWARD_STATE; // correct initialization? right_motor_speed = 0; left_motor_speed = 0; create_thread(&claws_operation_thread, STACK_DEFAULT, 5, "claws"); claws_thread_on = 1; claws_state = CLAWS_CLOSED; return 0; } // Entry point to contestant code. // Create threads and return 0. int umain (void) { assignment4(); pause(3000); spin_using_gyro(125, DEFAULT_TIMEOUT); while(1) { open_claws(); drive_straight(FIVE_FEET, CLAWS_CLOSED); spin_using_gyro(LEFT90, DEFAULT_TIMEOUT); //drive_straight(THREE_FEET, CLAWS_CLOSED); } return 0; } /* MOTOR FUNCTIONS */ // for use with tagged motors (currently only the two drive motors: 19/1/09) // takes a motor, changes its speed by the increment step, and updates the motor's speed tag // updated to avoid overflow (24-1-09) uint8_t change_motor_speed(int motor, int16_t step) { switch(motor) { case RIGHT_MOTOR: right_motor_speed += step; if(right_motor_speed > 255) right_motor_speed = 255; else if(right_motor_speed < -255) right_motor_speed = -255; motor_set_vel(motor, right_motor_speed); break; case LEFT_MOTOR: left_motor_speed += step; if(left_motor_speed > 255) left_motor_speed = 255; else if(left_motor_speed < -255) left_motor_speed = -255; motor_set_vel(motor, left_motor_speed); break; } return 0; } // for use with tagged motors // takes a motor, sets its speed, and updates its speed tag uint8_t set_motor_speed(int motor, int16_t speed) { if(speed == DRIVE_BRAKE) { switch(motor) { case RIGHT_MOTOR: right_motor_speed = 0; motor_brake(RIGHT_MOTOR); break; case LEFT_MOTOR: left_motor_speed = 0; motor_brake(LEFT_MOTOR); break; } } else { switch(motor) { case RIGHT_MOTOR: right_motor_speed = speed; motor_set_vel(RIGHT_MOTOR, right_motor_speed); break; case LEFT_MOTOR: left_motor_speed = speed; motor_set_vel(LEFT_MOTOR, left_motor_speed); break; } } return 0; } // may need to be changed later // starts the two drive motors using speed tags (19/1/09) // edited out the revving up (21/1/09) void start_drive_motors() { /* for(int i = 1; i <= 5; i++) { change_motor_speed(RIGHT_MOTOR, GOAL_MOTOR_SPEED_RIGHT / 10); change_motor_speed(LEFT_MOTOR, GOAL_MOTOR_SPEED_LEFT / 10); pause(30); } */ set_motor_speed(RIGHT_MOTOR, GOAL_MOTOR_SPEED_RIGHT); set_motor_speed(LEFT_MOTOR, GOAL_MOTOR_SPEED_LEFT); } // sets the two drive motors to coast using speed tagging (19/1/09) void coast_drive_motors() { set_motor_speed(RIGHT_MOTOR, 0); set_motor_speed(LEFT_MOTOR, 0); } void stop_drive_motors() { set_motor_speed(RIGHT_MOTOR, DRIVE_BRAKE); set_motor_speed(LEFT_MOTOR, DRIVE_BRAKE); } void start_reverse_motors() { set_motor_speed(LEFT_MOTOR, MOTOR_SPEED_REVERSING_LEFT); set_motor_speed(RIGHT_MOTOR, MOTOR_SPEED_REVERSING_RIGHT); } uint8_t drive_straight(uint8_t num_encodings, uint8_t claws_end_state) { //if(toggle_claws) // claws_state = CLAWS_OPEN; drive_starting_angle = gyro_get_degrees(); // debug? reset_sencoders(); drive_state = FORWARD_STATE; drive_thread_on = 1; start_drive_motors(); while((average_sencodings() < num_encodings) && (drive_state == FORWARD_STATE)) { //printf("\nR: %d, L: %d", right_motor_speed, left_motor_speed); // debug pause(50); //printf("\nAvg. Enc.: %d",average_sencodings()); // debug: remove this asap } if(drive_state != FORWARD_STATE) { return MOTOR_SPIKE_ERROR; } else { //if(toggle_claws) // claws_state = CLAWS_CLOSED; claws_state = claws_end_state; drive_thread_on = 0; stop_drive_motors(); } //pause(200); // necessary? return 0; } // want to kill motor current spike thread here? or perhaps alter thread to check for reverse? // // returns the old claws state uint8_t drive_reverse(uint8_t num_encodings) { uint8_t old_claws_state = claws_state; drive_starting_angle = gyro_get_degrees(); // debug? reset_sencoders(); drive_state = REVERSE_STATE; drive_thread_on = 1; start_reverse_motors(); claws_state = CLAWS_CLOSED; // make sure this works properly //claws_state = CLAWS_RECLOSE; while((average_sencodings() < num_encodings) && (drive_state == REVERSE_STATE)) { pause(50); } if(drive_state != REVERSE_STATE) { return MOTOR_SPIKE_ERROR; } drive_thread_on = 0; stop_drive_motors(); return old_claws_state; } // need semaphores or some shit? // unfinished int drive_straight_thread() { uint16_t left_encodings; //= encoder_read(LEFT_SENCODER); uint16_t right_encodings; //= encoder_read(RIGHT_SENCODER); float degrees_off = 0.0; while(1) { while(drive_thread_on) //stay on { left_encodings = encoder_read(LEFT_SENCODER); right_encodings = encoder_read(RIGHT_SENCODER); int16_t diff = left_encodings - right_encodings; degrees_off = gyro_get_degrees() - drive_starting_angle; switch(drive_state) { case FORWARD_STATE: if(degrees_off < -1.5) { //go left change_motor_speed(LEFT_MOTOR, -MINOR_SPEED_ADJUST_INCREMENT); //printf("\nGyro: Left"); // debug pause(50); } else if(degrees_off > 1.5) { //go right change_motor_speed(LEFT_MOTOR, MINOR_SPEED_ADJUST_INCREMENT); //printf("\nGyro: Right"); // debug pause(50); } if(diff <= -6) //else if(diff <= -6) { //turn right change_motor_speed(LEFT_MOTOR, MAJOR_SPEED_ADJUST_INCREMENT); // write this in later //drive_state = ADJUST_RIGHT; //enter ADJUST_RIGHT } else if(diff >= 6) { //turn left //change_motor_speed(RIGHT_MOTOR, MAJOR_SPEED_ADJUST_INCREMENT); change_motor_speed(LEFT_MOTOR, -MAJOR_SPEED_ADJUST_INCREMENT); // write this in later //drive_state = ADJUST_LEFT; //enter ADJUST_LEFT } else if(diff >= 2) { //slight left change_motor_speed(LEFT_MOTOR, -MINOR_SPEED_ADJUST_INCREMENT); } else if(diff <= -2) { //slight right change_motor_speed(LEFT_MOTOR, MINOR_SPEED_ADJUST_INCREMENT); } break; case ADJUST_LEFT: //write later return 1; break; case ADJUST_RIGHT: //write later return 2; break; case REVERSE_STATE: if(degrees_off < -1.5) { //go "right" (oriented toward front) change_motor_speed(LEFT_MOTOR, -MINOR_SPEED_ADJUST_INCREMENT); //printf("\nGyro: Left"); // debug pause(50); } else if(degrees_off > 1.5) { //go "left" change_motor_speed(LEFT_MOTOR, MINOR_SPEED_ADJUST_INCREMENT); //printf("\nGyro: Right"); // debug pause(50); } if(diff <= -6) //else if(diff <= -6) { //turn "right" change_motor_speed(LEFT_MOTOR, -MAJOR_SPEED_ADJUST_INCREMENT); } else if(diff >= 6) { //turn "left" change_motor_speed(LEFT_MOTOR, MAJOR_SPEED_ADJUST_INCREMENT); } else if(diff >= 2) { //slight "left" change_motor_speed(LEFT_MOTOR, MINOR_SPEED_ADJUST_INCREMENT); } else if(diff <= -2) { //slight "right" change_motor_speed(LEFT_MOTOR, -MINOR_SPEED_ADJUST_INCREMENT); } break; case TURN_STATE: //temporarily do nothing break; case MOTOR_SPIKE_STATE: pause(50); break; //printf("\n"); } //if(right_motor_speed > 20) // printf("\nR: %d, L: %d", right_motor_speed, left_motor_speed); // debug pause(100); } drive_starting_angle = gyro_get_degrees(); // correct placement??? yield(); //pause(200); // eh? } return 0; } /* CLAWS CODE */ uint8_t move_claws(int16_t speed, uint16_t max_time) { //insert check against move_claws(0) uint16_t final_time = get_time() + max_time; motor_set_vel(CLAWS_MOTOR, speed); while((motor_get_current_MA(CLAWS_MOTOR) < CLAWS_MOTOR_THRESHOLD) && (get_time() < final_time)) { pause(5); } motor_brake(CLAWS_MOTOR); // nothing for claws_position... return 0; } uint8_t close_claws() { move_claws(MOTOR_SPEED_CLAWS_CLOSE, CLAWS_TIMEOUT); claws_position = CLAWS_CLOSED; return 0; } uint8_t open_claws() { move_claws(MOTOR_SPEED_CLAWS_OPEN, CLAWS_TIMEOUT); claws_position = CLAWS_OPEN; return 0; } uint8_t goalify_claws() { if(claws_position == CLAWS_CLOSED) { move_claws(MOTOR_SPEED_CLAWS_OPEN, CLAWS_GOAL_TIMEOUT); claws_position = CLAWS_GOAL; } else if(claws_position == CLAWS_OPEN) { close_claws(); goalify_claws(); } else if(claws_position == CLAWS_GOAL) { return 1; } else return 2; return 0; } uint8_t reclose_claws() { move_claws(MOTOR_SPEED_CLAWS_CLOSE, CLAWS_GOAL_TIMEOUT); claws_position = CLAWS_CLOSED; return 0; } // thread for claws operation // possibly could be combined with sensors in future? int claws_operation_thread() { //claws_state = CLAWS_CLOSED; //close_claws(); while(1) { while(claws_thread_on) // could become part when claws operation using sensing? ...maybe change later { switch(claws_state) { case CLAWS_OPEN: if(claws_position != CLAWS_OPEN) open_claws(); break; case CLAWS_CLOSED: if(claws_position != CLAWS_CLOSED) close_claws(); break; case CLAWS_GOAL: if(claws_position != CLAWS_GOAL) goalify_claws(); break; case CLAWS_RECLOSE: reclose_claws(); pause(50); // eh? break; default: break; } pause(10); } yield(); //pause(200); // eh? } return 0; } /* DISTANCE SENSOR CODE */ // returns a nonnegative integer based on readings from num_sens distance sensors // order of dist. sens.: front (1), rear (2), left (4), right (8) uint8_t read_dist_sens_digital(uint8_t num_sens) { uint8_t scan_result = 0; for(uint8_t i = 0; i < num_sens; i++) // order: front, rear, left, right dist. sens. { if(analog_read(FRONT_DIST_SENS + i) > DIST_SENS_ORIENT_THRESHOLD) { scan_result += (1 << i); } } return scan_result; } /* SHAFT ENCODER CODE */ void reset_sencoders() { encoder_reset(LEFT_SENCODER); encoder_reset(RIGHT_SENCODER); } int16_t average_sencodings() { return (encoder_read(LEFT_SENCODER) + encoder_read(RIGHT_SENCODER)) / 2; } // change to math.abs (or similar) if necessary int int_abs(int a) { if(a < 0) return -a; return a; }