ROBOTC.net forums
http://www.robotc.net/forums/

Is there a library of useful functions anywhere?
http://www.robotc.net/forums/viewtopic.php?f=11&t=705
Page 1 of 1

Author:  fryfrog [ Sat Aug 30, 2008 2:31 am ]
Post subject:  Is there a library of useful functions anywhere?

So I've had my Vex for a while and I just this evening purchased RobotC and a few new Vex parts. I'm ready to start playing around (I'm 30 years old, so not in any competitions or anything), but I thought I'd see if I needed to re-invent the wheel or something.

All of the demo code for RobotC is very specific purpose, but I've been thinking I'd like to start by creating some general use code.

For example, given the diameter of driven wheels, gear reduction and wheel base it should be pretty easy to create a few useful functions such as...

drive straight X meters at speed Y (given encoders)
point turn X degrees at speed Y (also given encoders)
anchor (wide?) turn X degrees at speed Y (same thing here)

Has anyone done something like this yet? My google searches are not turning up much useful information.

Author:  fryfrog [ Sat Aug 30, 2008 5:17 am ]
Post subject:  Re: Is there a library of useful functions anywhere?

Here is what I have so far. I'm having a hard time with the turning X degrees because I suck at math. I just need to figure out how to take the distance traveled (measured by the sensors), compare that with the known turning circle (because we know the wheel base) and then turn that into degrees.

For example, a 10cm wheel base with 3cm wheels means traveling ~5cm is 90 degrees. How do I make that into an equation! I'll figure it out i'm sure, but if anyone wants to take a look at what I'm doing so far... feed back is welcome.

It compiles, but I haven't plugged it into any robot.

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);
   }
}

Page 1 of 1 All times are UTC - 5 hours [ DST ]
Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group
http://www.phpbb.com/