Code: #pragma config(Sensor, in15, leftEncoder, sensorQuadEncoder, int5) #pragma config(Sensor, in16, rightEncoder, sensorQuadEncoder, int6) #pragma config(Motor, port1, leftMotor, tmotorNormal) #pragma config(Motor, port2, rightMotor, tmotorNormal) //*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
/*****************************************************************************/ /* Title: Useful Robot Movement Functions for Skid Steer Robots */ /* Description: A collection of functions designed to provide useful */ /* locomotion to a robot that uses skid steering to drive. */ /* All length measurements are in centimeters and speed in -127 to 127 */ /* For proper use, motors and encoders should be named as follows: */ /* leftMotor, rightMotor, leftEncoder, rightEncoder */ /*****************************************************************************/
// Outside diameter of driven wheel (in cm), used to determine circumfrence. int WheelDiameter = 5; // Outside distance between driven wheels (in cm), used to determine turning circle. int WheelBase = 10; // Useful when encoder is not hooked directly to driven wheel. int GearRatio = 1; // Speed fuzziness, used to keep us going straight (will be doubled in use). int SpeedFuzz = 1; // Sleep time, used for turning and going straight. int SleepTime = 500;
int DistancePerTick = (3.14 * WheelDiameter)/90 * GearRatio; int TurningCircle = (3.14 * WheelBase);
void DriveStraight(); void DriveStraightDistance (int distance, int speed); void DriveStraightTime (int time, int speed); void PointTurnDegrees (int degree, int direction, int speed); void WideTurnDegrees (int degree, int direction, int speed);
task main() { // Go 10 cm at 100 speed. DriveStraightDistance(10,100); // Go 2 seconds at 100 speed. DriveStraightTime(2,100); // Pivot 90 degrees right (cw) at 100 speed. PointTurnDegrees(90, 1, 100); // Wide turn 90 degrees left (ccw) at 100 speed. WideTurnDegrees(90, -1, 100); }
void DriveStraight() { // Left side going faster than right. if (SensorValue[leftEncoder] > SensorValue[rightEncoder]) { motor[leftMotor] -= SpeedFuzz; motor[rightMotor] += SpeedFuzz; } // Left side going slower than right. if (SensorValue[leftEncoder] < SensorValue[rightEncoder]) { motor[leftMotor] += SpeedFuzz; motor[rightMotor] -= SpeedFuzz; } }
// Drive straight given distance (in cm) at speed (-127 to 127). void DriveStraightDistance (int distance, int speed) { // Zero out variables and sensors int traveled = 0; SensorValue[leftEncoder] = 0; SensorValue[rightEncoder] = 0;
// Lets get moving! motor[leftMotor] = speed; motor[rightMotor] = speed;
while(traveled <= distance) { DriveStraight(); wait1Msec(SleepTime); // Average the distance traveled by adding both sensors and dividing by 2. traveled = (SensorValue[leftEncoder] + SensorValue[rightEncoder])/2 * DistancePerTick; } }
// Drive straight for time (in 1/100ths of a second at speed (-127 to 127). void DriveStraightTime (int time, int speed) { // Zero out variables, sensors and timers ClearTimer(T1); SensorValue[leftEncoder] = 0; SensorValue[rightEncoder] = 0;
// Lets get moving! motor[leftMotor] = speed; motor[rightMotor] = speed;
while(time100[T1] <= time) { DriveStraight(); wait1Msec(SleepTime); } }
// Point turn of degrees in direction at speed. // -1 is left (ccw), 1 is right (cw) // speed should be 0 to 127 void PointTurnDegrees (int degree, int direction, int speed) { // Zero out variables and sensors int turned = 0; int traveled = 0; SensorValue[leftEncoder] = 0; SensorValue[rightEncoder] = 0;
// Lets get moving! motor[leftMotor] = speed * direction; motor[rightMotor] = speed * direction * -1;
while (turned <= degree) { DriveStraight(); traveled = (SensorValue[leftEncoder] + SensorValue[rightEncoder]) * DistancePerTick; turned = (traveled / TurningCircle); wait1Msec(SleepTime); } }
// Wide turn of degrees in direction at speed. // -1 is left, 1 is right // speed should be 0 to 127 void WideTurnDegrees (int degree, int direction, int speed) { // Zero out variables and sensors int turned = 0; int traveled = 0; SensorValue[leftEncoder] = 0; SensorValue[rightEncoder] = 0;
// Turning left, stop left motor and power up right. if (direction == -1) { motor[rightMotor] = speed; motor[leftMotor] = 0; }
// Turning right, stop right motor and power up left. if (direction == 1) { motor[rightMotor] = 0; motor[leftMotor] = speed; }
while (turned <= degree) { traveled = (SensorValue[leftEncoder] + SensorValue[rightEncoder]) * DistancePerTick; turned = (traveled / TurningCircle); wait1Msec(SleepTime); } }
|