/** * Turns bot left only using left side motors * * @warning requires gyro * * @author Bernard Suwirjo [email protected] * @author Sean Kelley [email protected] * * @param degrees amount of degrees to turn left * @param forward boolean if bot is turning forward or backward * @param speed speed of motors * */ void fancyTurnLeftDegrees(int degrees, bool forward=true, int speed = MOTOR_SPEED){ // reset encoders degrees=degrees*10; // reset gyro //gyro takes degrees from 0-3600, so we multiply by 10 to get a gyro processable number SensorValue[gyro]=0; // turn forwards or backwards based on forward boolean if(forward){ while(abs(SensorValue[gyro]) < degrees){ //While the gyro value is less than the target perform code below //Set only the left side motors to the target value frontRightVal = speed; backRightVal = speed; } // stop motors clearMotors(); } else { while(abs(SensorValue[gyro]) < degrees){ //Set only the left side motors to the negative target value frontRightVal = -speed; backRightVal = -speed; } // stop motors clearMotors(); } }
/** * Runs each motor for 1.5 seconds * * @author Bernard Suwirjo [email protected] */ void testMotors() { frontRightVal=118;//Set individual motor wait1Msec(1500); //Wait 1.5 seconds clearMotors(); //clear motor(s) backRightVal=118; wait1Msec(1500); clearMotors(); frontLeftVal=118; wait1Msec(1500); clearMotors(); backLeftVal=118; wait1Msec(1500); clearMotors(); }
/** * Turns bot left a given amount of degrees * * @author Bernard Suwirjo [email protected] * @author Sean Kelley [email protected] * * @param degree amount of degrees to turn left * @param speed speed of motors * */ void turnLeftDegrees(float degree, float speed=90) { //Reset gyro SensorValue[gyro]=0; //gyro takes degrees from 0-3600, so we multiply by 10 to get a gyro processable number degree=degree*10; //We want to slow down when we approach the target, so we calculate a first turn segment as 60% of the total float first=degree*.6; while(abs(SensorValue[gyro]) < first) { //Since it's turn left, we want to set right motors forwards and left motors backwards. motor[LD] = speed; motor[RD] = speed; } while(abs(SensorValue[gyro]) < degree) { //We don't want the motors to run too slow, so we set a a safety net. The motor can't have a power less than 40. if(speed*.35<40)//If 35% of the motor power is less than 40, set the power to 40. { motor[LD] = 40; motor[RD] = 40; } else { //If not set it to 35% motor[LD] = speed*.35; motor[RD] = speed*.35; } } while(abs(SensorValue[gyro]) > degree) { motor[LD] = 30; motor[RD] = 30; } clearMotors(); }
/** * Moves bot backward for a given amount of seconds * * @author Bernard Suwirjo [email protected] * * @param seconds amount of seconds to move backward * @param speed speed of motors * */ void backwardSeconds(float seconds, int speed=MOTOR_SPEED) { //Set all motors to negative target value setMotors(-speed); wait1Msec(seconds * 1000);//Wait given amount of time clearMotors(); }
/** * Turns bot right a given amount of degrees * * @warning requires gyro * * @author Bernard Suwirjo [email protected] * @author Sean Kelley [email protected] * * @param degree amount of degrees to turn right * @param speed speed of motors * */ void turnRightDegrees(float degree, float speed=90) { //Reset gyro SensorValue[gyro]=0; //gyro takes degrees from 0-3600, so we multiply by 10 to get a gyro processable number degree=degree*10; //We want to slow down when we approach the target, so we calculate a first turn segment as 60% of the total float first=degree*.6; while(abs(SensorValue[gyro]) < first){ //Turn the first 60% //Since it's turn right, we want to set right motors backwards and left motors forward. frontLeftVal = speed; frontRightVal = -speed; backLeftVal = speed; backRightVal = -speed; } while(abs(SensorValue[gyro]) <degree){ //Turn the remainin amount. //We don't want the motors to run too slow, so we set a a safety net. The motor can't have a power less than 40. if(speed*.35<40)//If 35% of the motor power is less than 40, set the power to 40. { frontLeftVal = 40; frontRightVal = -40; backLeftVal = 40; backRightVal = -40; } else { //If not set it to 35% frontLeftVal = speed*.35; frontRightVal = -speed*.35; backLeftVal = speed*.35; backRightVal = -speed*.35; } } clearMotors(); }
/** * Turns bot left a given amount of seconds * * @author Bernard Suwirjo [email protected] * * @param seconds amount of seconds to turn left * @param speed speed of motors * */ void turnLeftSeconds(float seconds, float speed=118) { //Since turn left, we want to set the right motors forward and the left motors backwards motor[LD] = -speed; motor[RD] = speed; wait1Msec(seconds*1000); //Wait desired amount of time clearMotors(); //Stop }
void setup() {
left.attach(2,1300,1700);
right.attach(13,1300,1700);
// attaches the servo on pin 9 to the servo object
left.write(90);
right.write(90);
turn(90);
clearMotors();
}
/** * Turns bot right a given amount of seconds * * @author Bernard Suwirjo [email protected] * * @param seconds amount of seconds to turn right * @param speed speed of motors * */ void turnRightSeconds(float seconds, float speed=118) { //Since turn right, we want to set left motors forwards and right motors backwards. frontLeftVal=speed; backLeftVal=speed; frontRightVal=-speed; backRightVal=-speed; wait1Msec(seconds*1000); //Wait desired amount of time clearMotors(); //Stop }
/** * Drive until an encoder value is reached * * @author Sean Kelley [email protected] * * @param encoder_count encoder ticks to drive forward * @param timeout_in_seconds timeout for motors if encoder value is surpassed * @param speed speed of motors * */ int driveByEncoder( int encoder_count, int timeout_in_seconds = 5 , int speed=MOTOR_SPEED) { int timeout; // Drive motor until encoder has moved a number counts or // timeout_in_seconds seconds have passed // Zero the encoder SensorValue[ encoder ] = 0; // Run the motor forwards or backwards if( encoder_count > 0 ) { setMotors(speed); } else { setMotors(-speed); } // run loop until encoder hits encoder_count counts or timeout reached for( timeout=(timeout_in_seconds*TIMEOUT_CNT_PER_SEC); timeout > 0; timeout-- ) { // check encoder if( encoder_count > 0 ) { // going forwards if( SensorValue[ encoder ] >= encoder_count ) { break; } else { // going backwards if( SensorValue[ encoder ] <= encoder_count ) { break; } } } // wait 1/10 second wait1Msec( 100 ); } // Stop the motor clearMotors(); // See if we sucessfully found the right encoder value if( timeout <= 0 ) { // there was an error - perhaps do something // return error return (-1); } else { // return success return 0; } }
