Permalink
Switch branches/tags
Nothing to show
Find file Copy path
cd245ff Sep 23, 2017
198 lines (157 sloc) 10.8 KB
#pragma config(Sensor, port11, touchLed, sensorVexIQ_LED)
#pragma config(Sensor, port5, gyro, sensorVexIQ_Gyro)
#pragma config(Motor, motor4, left, tmotorVexIQ, PIDControl, encoder)
#pragma config(Motor, motor10, right, tmotorVexIQ, PIDControl, reversed, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
// More info & documentation on robotsquare.com
//////////////////////////////////////////////////////////////////////////////////
///
/// HOW TO RUN THIS PROGRAM:
///
/// 1. Make sure you are using RobotC 4.54 or higher, and VEXos 2.0.1 or higher.
/// 2. Use VEXos to ensure that all your VEX IQ components are up to date.
/// 3. Put the segway upright, with its wheels resting on the ground.
/// 4. Don't hold it too tightly: just enough to prevent it from falling over.
/// 5. Start the program from the TeleOp Pgms menu. The Touch LED will turn blue.
/// 6. Keep holding the Segway upright until the Touch LED turns green.
/// 7. Let go. It should balance now.
/// 8. Let it balance in place or drive it around with the remote.
/// 9. If you touch the Touch LED again, it turns red and the program stops.
///
/// On your first try, keep your hands close just in case it falls.
/// If it falls, don't try to "help" it balance! It's better to restart from step 1.
///
///
//////////////////////////////////////////////////////////////////////////////////
task main()
{
// Initializing system state variables
float motorAngleRaw, // The angle of the "motor", measured in degrees. We will take the average of both motor positions, wich is essentially how far the middle of the robot has traveled.
motorAngle, // The angle of the motor, converted to radians (2*pi radians equals 360 degrees).
motorAngleReference = 0, // The reference angle of the motor. The robot will attempt to drive forward or backward, such that its measured position equals this reference (or close enough).
motorAngleError, // The error: the deviation of the measured motor angle from the reference. The robot attempts to make this zero, by driving toward the reference.
motorAngleErrorAccumulated = 0, // We add up all of the motor angle error in time. If this value gets out of hand, we can use it to drive the robot back to the reference position a bit quicker.
motorAngularSpeed, // The motor speed, estimated by how far the motor has turned in a given amount of time
motorAngularSpeedReference = 0, // The reference speed during manouvers: how fast we would like to drive, measured in radians per second.
motorAngularSpeedError, // The error: the deviation of the motor speed from the reference speed.
motorDutyCycle, // The 'voltage' signal we send to the motor. We calulate a new value each time, just right to keep the robot upright.
gyroRateRaw, // The raw value from the gyro sensor in rate mode.
gyroRate, // The angular rate of the robot (how fast it is falling forward), measured in radians per second.
gyroEstimatedAngle = 0, // The gyro doesn't measure the angle of the robot, but we can estimate this angle by keeping track of the gyroRate value in time.
gyroOffset = 0; // Over time, the gyro rate value can drift. This causes the sensor to think it is moving even when it is perfectly still. We keep track of this offset.
// Set the LED to blue while calibrating the gyro
setTouchLEDRGB(touchLed, 0, 0, 255);
//Set the motor brake mode
setMotorBrakeMode(right,motorBrake);
setMotorBrakeMode(left,motorBrake);
//Calculating the (initial) avrage gyro sensor value
const int gyroRateCalibrateCount = 100;
for (int i = 0; i < gyroRateCalibrateCount; i++){
gyroOffset = gyroOffset + getGyroRate(gyro);
sleep(10);
}
//The offset is equal to the long term average value of the sensor.
gyroOffset = gyroOffset/gyroRateCalibrateCount;
//Timing settings for the program
const int loopTimeMiliSec = 20, // Time of each loop, measured in miliseconds.
motorAngleHistoryLength = 4; // Number of previous motor angles we keep track of.
const float loopTimeSec = loopTimeMiliSec/1000.0, // Time of each loop, measured in seconds.
radiansPerDegree = PI/180.0, // The number of radians in a degree.
radiansPerSecondPerRawGyroUnit = radiansPerDegree, // For the VEX IQ, 1 unit = 1 deg/s
radiansPerRawMotorUnit = radiansPerDegree, // For the VEX IQ, 1 unit = 1 deg
gyroDriftCompensationRate = 0.1*loopTimeSec, // The rate at which we'll update the gyro offset
degPerSecPerPercentSpeed = 7.1, // On the VEX IQ, "1% speed" corresponds to 7.1 deg/s (if speed control were enabled)
radPerSecPerPercentSpeed = degPerSecPerPercentSpeed * radiansPerDegree; //Convert this number to the speed in rad/s per "percent speed"
//A (fifo) array which we'll use to keep track of previous motor positions, which we can use to calculate the rate of change (speed)
float motorAngleHistory[motorAngleHistoryLength];
memset(motorAngleHistory,0,4*motorAngleHistoryLength);
int motorAngleIndex = 0;
const float gainGyroAngle = 900, //For every radian (57 degrees) we lean forward, apply this amount of duty cycle.
gainGyroRate = 36 , //For every radian/s we fall forward, apply this amount of duty cycle.
gainMotorAngle = 15, //For every radian we are ahead of the reference, apply this amount of duty cycle
gainMotorAngularSpeed = 9.6, //For every radian/s drive faster than the reference value, apply this amount of duty cycle
gainMotorAngleErrorAccumulated = 3; //For every radian x s of accumulated motor angle, apply this amount of duty cycle
// Variables to control the reference speed and steering
float speed = 0,
steering = 0;
//Joystick positions
int joyStickLeft = 0,
joyStickRight = 0;
//Maximum speed and steering when remote joysticks are fully moved forward
const int kMaxRemoteSpeed = 20;
const int kMaxRemoteSteering = 10;
//Initialization complete
setTouchLEDRGB(touchLed, 0, 255, 0);
//Run the main loop until the touch LED is touched.
while(getTouchLEDValue(touchLed)==0)
{
///////////////////////////////////////////////////////////////
//
// Driving and Steering. Modify the <<speed>> and <<steering>>
// variables as you like to make your segway go anywhere!
//
// (If you don't have the controller, just delete the four lines
// below. Then <<speed>> and <<steering>> remain zero.)
//
///////////////////////////////////////////////////////////////
joyStickLeft = getJoystickValue(ChA);
joyStickRight = getJoystickValue(ChD);
speed = (joyStickRight + joyStickLeft)/2.0 * (kMaxRemoteSpeed /100.0);
steering = (joyStickRight - joyStickLeft)/2.0 * (kMaxRemoteSteering/100.0);
///////////////////////////////////////////////////////////////
// (You don't need to modify anything in the sections below.)
///////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////
// Reading the Gyro.
///////////////////////////////////////////////////////////////
gyroRateRaw = getGyroRateFloat(gyro);
gyroRate = (gyroRateRaw - gyroOffset)*radiansPerSecondPerRawGyroUnit;
///////////////////////////////////////////////////////////////
// Reading the Motor Position
///////////////////////////////////////////////////////////////
motorAngleRaw = (getMotorEncoder(right) + getMotorEncoder(left))/2.0;
motorAngle = motorAngleRaw*radiansPerRawMotorUnit;
motorAngularSpeedReference = speed*radPerSecPerPercentSpeed;
motorAngleReference = motorAngleReference + motorAngularSpeedReference*loopTimeSec;
motorAngleError = motorAngle - motorAngleReference;
///////////////////////////////////////////////////////////////
// Computing Motor Speed
///////////////////////////////////////////////////////////////
motorAngularSpeed = (motorAngle - motorAngleHistory[motorAngleIndex])/(motorAngleHistoryLength*loopTimeSec);
motorAngleHistory[motorAngleIndex] = motorAngle;
motorAngularSpeedError = motorAngularSpeed;
///////////////////////////////////////////////////////////////
// Computing the motor duty cycle value
///////////////////////////////////////////////////////////////
motorDutyCycle = gainGyroAngle * gyroEstimatedAngle
+ gainGyroRate * gyroRate
+ gainMotorAngle * motorAngleError
+ gainMotorAngularSpeed * motorAngularSpeedError
+ gainMotorAngleErrorAccumulated * motorAngleErrorAccumulated;
///////////////////////////////////////////////////////////////
// Apply the signal to the motor, and add steering
///////////////////////////////////////////////////////////////
setMotorSpeed(right, motorDutyCycle + steering);
setMotorSpeed(left, motorDutyCycle - steering);
///////////////////////////////////////////////////////////////
// Update angle estimate and Gyro Offset Estimate
///////////////////////////////////////////////////////////////
gyroEstimatedAngle = gyroEstimatedAngle + gyroRate*loopTimeSec;
gyroOffset = (1-gyroDriftCompensationRate)*gyroOffset+gyroDriftCompensationRate*gyroRateRaw;
///////////////////////////////////////////////////////////////
// Update Accumulated Motor Error
///////////////////////////////////////////////////////////////
motorAngleErrorAccumulated = motorAngleErrorAccumulated + motorAngleError*loopTimeSec;
motorAngleIndex = (motorAngleIndex + 1) % motorAngleHistoryLength;
///////////////////////////////////////////////////////////////
// Wait for the loop to complete
///////////////////////////////////////////////////////////////
sleep(loopTimeMiliSec);
}
///////////////////////////////////////////////////////////////
// Turn off the motors, and end the program.
///////////////////////////////////////////////////////////////
setTouchLEDRGB(touchLed, 255, 0, 0);
setMotorSpeed(left,0);
setMotorSpeed(right,0);
}