Add example code for modified Segway IQ as seen at VEX Worlds 2016

vex-iq/robotc/segway-line-follow-animated-eyes.c

@@ -0,0 +1,205 @@
+#pragma config(Sensor, port3,  touchLedRight,  sensorVexIQ_LED)
+#pragma config(Sensor, port5,  gyro,           sensorVexIQ_Gyro)
+#pragma config(Sensor, port9,  light,          sensorVexIQ_ColorGrayscale)
+#pragma config(Sensor, port11, touchLedLeft,   sensorVexIQ_LED)
+#pragma config(Sensor, port12, sonar,          sensorVexIQ_Distance)
+#pragma config(Motor,  motor4,          left,          tmotorVexIQ, PIDControl, encoder)
+#pragma config(Motor,  motor6,          eyes,          tmotorVexIQ, PIDControl, reversed, encoder)
+#pragma config(Motor,  motor10,         right,         tmotorVexIQ, PIDControl, reversed, encoder)
+//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
+
+//////////////////////////////////////////////////////////////////////////////////
+///
+/// This is an enhanced version of the Segway IQ, seen at VEX Worlds, with animatronic eyes and following a line
+///
+/// You can expand the base model and make it look like this: https://www.youtube.com/watch?v=8drvJdtlWAQ
+///
+//////////////////////////////////////////////////////////////////////////////////
+
+task main()
+{
+  // Initializing ystem 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(touchLedLeft, 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 = 1,              // For the VEX IQ, 1 unit = 1 rad/s
+              radiansPerRawMotorUnit = radiansPerDegree,       // For the VEX UQ, 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) 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                  = 1000, //For every radian (57 degrees) we lean forward,            apply this amount of duty cycle.
+              gainGyroRate                   = 40,   //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;
+
+   float lightErr = 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(touchLedLeft, 0, 0, 255);
+   setTouchLEDRGB(touchLedRight, 0, 0, 255);
+
+ 	 //Program runtime
+   TTimers RunTime;
+   clearTimer(RunTime);
+
+   //Color Sensor
+   const int kWhiteColor = 1200/4;
+   const int kBlackColor = 160/4;
+   const int kLightThreshold = (kWhiteColor + kBlackColor)/2;
+
+   //Run the main loop until the touch LED is touched.
+   while(getTouchLEDValue(touchLedLeft)==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.)
+      //
+      //  In this case, we set speed and steering as if we were making
+      //  a normal proportional line following robot
+      //
+      ///////////////////////////////////////////////////////////////
+
+      speed = 20;// Average forward speed
+      lightErr = (getColorGrayscale(light)-kLightThreshold)/100.0; // Deviation from the grey color in between black and white
+      steering = lightErr*14; // Set the steering level as the deviation from grey times a constant. A higher value results in tighter turns, a lower value means less tight turns
+
+      // Make a reference for the eyes to follow, and make the motor follow it with a proportional controller
+      float eyesRef = sin(getTimerValue(RunTime)/1000.0);
+      setMotorSpeed(eyes,(eyesRef*230 - getMotorEncoder(eyes))*2);
+
+      ///////////////////////////////////////////////////////////////
+      //  (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(touchLedLeft, 255, 0, 0);
+   setMotorSpeed(left,0);
+   setMotorSpeed(right,0);
+
+}