View unanswered posts | View active topics It is currently Wed Sep 17, 2014 2:01 am






Reply to topic  [ 2 posts ] 
Is there a library of useful functions anywhere? 
Author Message
Rookie

Joined: Sat Aug 30, 2008 2:25 am
Posts: 25
Post 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.


Sat Aug 30, 2008 2:31 am
Profile
Rookie

Joined: Sat Aug 30, 2008 2:25 am
Posts: 25
Post 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);
   }
}


Sat Aug 30, 2008 5:17 am
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 2 posts ] 

Who is online

Users browsing this forum: No registered users and 1 guest


You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot post attachments in this forum

Search for:
Jump to:  



Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group.
Designed by ST Software for PTF.