diff --git a/vex-iq/robotc/segway-line-follow-animated-eyes.c b/vex-iq/robotc/segway-line-follow-animated-eyes.c new file mode 100644 index 0000000..cfc859b --- /dev/null +++ b/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 <> and <> + // variables as you like to make your segway go anywhere! + // + // (If you don't have the controller, just delete the four lines + // below. Then <> and <> 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); + +}