task main()
{
resetMotorEncoder(LeftMotor);
resetMotorEncoder(RightMotor);
while(true) {
while(getMotorEncoder(LeftMotor) < 500) {
setMotorSpeed(LeftMotor, 50);
setMotorSpeed(RightMotor, -50);
}
setMotorSpeed(LeftMotor, 0);
setMotorSpeed(RightMotor, 0);
delay(1000);
while(getMotorEncoder(LeftMotor) > 0) {
setMotorSpeed(LeftMotor, -50);
setMotorSpeed(RightMotor, 50);
}
setMotorSpeed(LeftMotor, 0);
setMotorSpeed(RightMotor, 0);
delay(1000);
}
}
task displayMyMotorPositions(){
float clawEncoderValue, wristEncoderValue, armEncoderValue, calibratedGyroDegrees, calibratedGyroHeading;
float leftMotorValue, rightMotorValue;
while(true) {
calibratedGyroDegrees = getGyroDegreesFloat(gyro);
calibratedGyroHeading = getGyroHeadingFloat(gyro);
clawEncoderValue = getMotorEncoder(clawMotor);
wristEncoderValue = getMotorEncoder(wristMotor);
armEncoderValue = getMotorEncoder(armMotor);
leftMotorValue = getMotorEncoder(leftMotor);
rightMotorValue = getMotorEncoder(rightMotor);
eraseDisplay();
displayString(0, "claw: %f3", clawEncoderValue);
displayString(1, "wrist: %f3", wristEncoderValue);
displayString(2, "arm: %f3", armEncoderValue);
displayString(3, "L: %f3 R: %f3", leftMotorValue, rightMotorValue);
displayString(4, "d: %f3 h: %f3", calibratedGyroDegrees, calibratedGyroHeading);
wait1Msec(1000);
}
}
//Activates motors required to vertically move
//@param encoderCounts change in counts
void driveEncoderVertical(int encoderCounts){
int direction = (encoderCounts > 0) ? 1 : -1;
int initialEncoder = getMotorEncoder(backRight);
while (abs(getMotorEncoder(backRight) - initialEncoder) < abs(encoderCounts)){
verticalMotors(direction);
}
stopDrive();
}
void driveToTarget(int target) {
displayTextLine(1, "%d", target);
resetMotorEncoder(LWheel);
resetMotorEncoder(RWheel);
setMotorSpeed(RWheel, 15);
setMotorSpeed(LWheel, 15);
while (getMotorEncoder(LWheel) < target || getMotorEncoder(RWheel) < target) {}
}
void rotateL (int speed, int forHowLong)
{
do
{
motor[left]=-speed;
motor[right]=speed;
}
while(getMotorEncoder(left)>-forHowLong && getMotorEncoder(right)<forHowLong);
motor[left] = motor[right] = 0;
}
void finalMethod(){
int masterPower = 30;
int slavePower = 30;
int error = 0;
//float error = 0;
//float kp = 0.2; -- float didnt work to good
int kp = 6;//initial value was 5, adjusted to get a better straight line
//changed to flaoting point numbers as ev3 does have floating point (original code was for a different system that only supported integers
resetMotorEncoder(motorB); //sets both motor 'encoders' (distance recorders) to zero
resetMotorEncoder(motorC);
//Repeat ten times a second.
while(getUSDistance(S4)> 10)// changed to 10 is probably safer
//while(getTouchValue (S1) || getTouchValue (S2))
{
//Set the motor powers to their respective variables.
setMotorSpeed(motorB, masterPower);
setMotorSpeed(motorC, slavePower);
error = getMotorEncoder(motorB)- getMotorEncoder(motorC);
slavePower += error / kp;
//slavePower = error * kp;
//Reset the encoders every loop so we have a fresh value to use to calculate the error.
resetMotorEncoder(motorB);
resetMotorEncoder(motorC);
}
while(!getTouchValue (S1) || !getTouchValue (S2) ){
sleep(50);
}
setMotorSpeed(motorB, 100);
setMotorSpeed(motorC, 100);
sleep(3000);
setMotorSpeed(motorB, 0);
setMotorSpeed(motorC, 0);
}
//auton methods//////////////////////////
/////////////////////////////////////////
void drive(int speed, float forHowLong)
{
float distance = forHowLong*156.8;
do
{
motor[left]=speed;
motor[right]=speed;
}
while(getMotorEncoder(left)< distance && getMotorEncoder(right)<distance);
motor[left] = motor[right] = 0;
}
//Activates the motors required to strafe robot
//@param direction: 1 is right, -1 is left
void sideMotors(int encoderCounts){
int direction = (encoderCounts > 0) ? 1 : -1;
int initialEncoder = getMotorEncoder(backRight);
while (abs(getMotorEncoder(backRight) - initialEncoder) < abs(encoderCounts)){
motor[frontLeft] = -1 * direction * 127;
motor[frontRight] = direction * 127;
motor[backLeft] = direction * 127;
motor[backRight] = -1 * direction * 127;
}
stopDrive();
}
void rotateL (int speed, int forHowLong)
{
float distance = forHowLong*mult;
do
{
motor[left]=-speed;
motor[right]=speed;
}
while(getMotorEncoder(left) > -distance && getMotorEncoder(right) < distance);
motor[left] = motor[right] = 0;
}
void steerRight()
{
if (getMotorEncoder(STEERING) > -steeringFactor) //"Turn Right" Operation
{
setMotorSpeed(STEERING, -50);
waitUntil(getMotorEncoder(STEERING) < -steeringFactor);
setMotorSpeed(STEERING, 0);
}
else
{
setMotorSpeed(STEERING, 0);
}
}
void steerLeft()
{
if (getMotorEncoder(STEERING) < steeringFactor) //"Turn Left" Operation
{
setMotorSpeed(STEERING, 50);
waitUntil(getMotorEncoder(STEERING) > steeringFactor);
setMotorSpeed(STEERING, 0);
}
else
{
setMotorSpeed(STEERING, 0);
}
}
void armAuto (int speed, int forHowLong)
{
do
{
motor [armL1] = speed;
motor [armL2] = speed;
motor [armL3] = speed;
motor [armR1] = speed;
motor [armR2] = speed;
motor [armR3] = speed;
}
while(getMotorEncoder(armL3)<forHowLong && getMotorEncoder(armR3)<forHowLong);
motor[left] = motor[right] = 0;
}
void driveStraightForward(){
long timeIn1mSec;
float leftMotorValue, rightMotorValue, drivePowerLevel_F, currentPowerLevel_F;
int drivePowerLevel, currentPowerLevel;
driveBackwardPressed = false;
if (!driveForwardPressed) {
clearTimer(T1);
driveForwardPressed = true;
resetMotorEncoder(leftMotor);
resetMotorEncoder(rightMotor);
}
leftMotorValue = getMotorEncoder(leftMotor);
rightMotorValue = getMotorEncoder(rightMotor);
drivePowerLevel = 100;
drivePowerLevel_F = (float) drivePowerLevel;
timeIn1mSec = time1(T1);
if ( timeIn1mSec < 50) currentPowerLevel_F = drivePowerLevel_F / 4.0;
else if ( timeIn1mSec < 100) currentPowerLevel_F = drivePowerLevel_F / 2.0;
else if ( timeIn1mSec < 150) currentPowerLevel_F = drivePowerLevel_F * (3 / 4);
else currentPowerLevel_F = drivePowerLevel_F;
currentPowerLevel = (int) currentPowerLevel_F;
if ( (abs(leftMotorValue) - 3) > abs(rightMotorValue) ) {
setMotorSpeed(rightMotor, currentPowerLevel);
currentPowerLevel = (int) ( currentPowerLevel_F * .94);
setMotorSpeed(leftMotor, currentPowerLevel);
}
else if ( (abs(rightMotorValue) - 3) > abs(rightMotorValue) ) {
setMotorSpeed(leftMotor, currentPowerLevel);
currentPowerLevel = (int) ( currentPowerLevel_F * .94);
setMotorSpeed(rightMotor, currentPowerLevel);
}
else {
setMotorSpeed(leftMotor, currentPowerLevel);
setMotorSpeed(rightMotor, currentPowerLevel);
}
}
task main()
{
int distanceBack = 1;
if (getBumperValue(bumpSwitch) = 1)
{
int rightEncoder = getMotorEncoder(rightMotor);
while ((getMotorEncoder(rightMotor)) > (getMotorEncoder(rightMotor)-distanceBack))
{
moveBackward(SLOW);
}
void armClose();
}
}
void steerRecenter()
{
if (getMotorEncoder(STEERING) == 0) // "Steer Recenter" Operation
{
setMotorSpeed(STEERING, 0);
}
else if (getMotorEncoder(STEERING) > 3)
{
setMotorSpeed(STEERING, -50);
waitUntil(getMotorEncoder(STEERING) < 3);
setMotorSpeed(STEERING, 0);
}
else if (getMotorEncoder (STEERING) < -3)
{
setMotorSpeed(STEERING, 50);
waitUntil(getMotorEncoder(STEERING) > -3);
setMotorSpeed(STEERING, 0);
}
}
void side(int degrees) {
setMotorSpeed(RWheel, 8);
setMotorSpeed(LWheel, 8);
resetMotorEncoder(motorA);
resetMotorEncoder(motorC);
int x = getMotorEncoder(motorA);
while (x < degrees) {
x = getMotorEncoder(motorA)
}
}
/// drive straight using the gyro
void gyroDriveInches(float inches, int driveSpeed) {
inches = inches-2;
float kP = 2.5;
bool onTarget = false;
resetMotorEncoder(leftMotor);
resetMotorEncoder(rightMotor);
resetGyro(gyro);
while(!onTarget) {
if(getMotorEncoder(leftMotor) < inches/7.8) {
setMotorSpeed(leftMotor, driveSpeed + proportionalOutput(kP, 0, getGyroDegreesFloat(gyro)));
}else {
onTarget = true;
}
if(getMotorEncoder(rightMotor) < inches/7.8) {
setMotorSpeed(rightMotor, driveSpeed - proportionalOutput(kP, 0, getGyroDegreesFloat(gyro)));
}
}
setMotorSpeed(leftMotor, 0);
setMotorSpeed(rightMotor, 0);
sleep(200);
}
task main()
{
//Initialize System
motor[armMotor] = OFF;
resetMotorEncoder(leftMotor);
resetMotorEncoder(armMotor);
while(true){
// Used to Manually Adjust Height of Arm (Testing Purposes)
while(SensorValue(leftButton)){
motor[armMotor] = -40;
}
while(SensorValue(rightButton)){
motor[armMotor] = 40;
}
motor[armMotor] = 0;
switch(state){
case stopped:
motor[leftMotor] = OFF;
motor[rightMotor] = OFF;
motor[armMotor] = OFF;
break;
case searching:
motor[rightMotor] = motorForwards;
motor[leftMotor] = motorForwards;
clearTimer(T1);
high = 0;
low = 4096;
while(time1[T1] < searchPeriod){
if(SensorValue(sonarSensor) > wallDistance){
wallDistance = SensorValue(sonarSensor);
}//end if
if(SensorValue(InfraCollector) > high){
high = SensorValue(InfraCollector);
}else if(SensorValue(InfraCollector) < low){
low = SensorValue(InfraCollector);
}// end if else
}//end while
if((high - low) > signalThreshhold){
state = forwards;
}else if(getMotorEncoder(leftMotor) > 2000){
state = moveCloser;
}else{
state = searching;
}//end if else
break;
case forwards:
resetMotorEncoder(leftMotor);
while(SensorValue(sonarSensor) > 35){
motor[leftMotor] = -35;
motor[rightMotor] = 35;
}//end while
resetMotorEncoder(armMotor);
motor[leftMotor] = 0;
motor[rightMotor] = 0;
if(getMotorEncoder(leftMotor) < -1250){
state = refine;
}else{
state = align;
}//end if else
break;
case moveCloser:
while(SensorValue(sonarSensor) < 170){
motor[leftMotor] = motorForwards;
motor[rightMotor] = motorForwards;
}//end while
resetMotorEncoder(leftMotor);
while(getMotorEncoder(leftMotor) > -1000){
motor[leftMotor] = motorBackwards;
motor[rightMotor] = motorForwards;
}//end while
motor[leftMotor] = OFF;
motor[rightMotor] = OFF;
resetMotorEncoder(leftMotor);
state = searching;
break;
case refine:
minDistance = 100;
resetMotorEncoder(leftMotor);
while(getMotorEncoder(leftMotor) < 200){
motor[leftMotor] = motorForwards;
motor[rightMotor] = motorForwards;
if(SensorValue(sonarSensor) < minDistance){
minDistance = SensorValue(sonarSensor);
}//end if
}//end while
resetMotorEncoder(leftMotor);
while(getMotorEncoder(leftMotor) > -400){
motor[leftMotor] = motorBackwards;
motor[rightMotor] = motorBackwards;
if(SensorValue(sonarSensor) < minDistance){
minDistance = SensorValue(sonarSensor);
}//end if
}//end while
resetMotorEncoder(leftMotor);
while(getMotorEncoder(leftMotor) < 400){
motor[leftMotor] = motorForwards;
motor[rightMotor] = motorForwards;
if(minDistance == SensorValue(sonarSensor)){
state = align;
break;
}//end if
}//end while
motor[leftMotor] = OFF;
motor[rightMotor] = OFF;
break;
case align:
while(SensorValue(sonarSensor) > 5){
motor[leftMotor] = -20;
motor[rightMotor] = 20;
}//end while
state = armUp;
break;
case armUp:
motor[leftMotor] = 0;
motor[rightMotor] = 0;
clearTimer(T1);
while(time1[T1] < armMovementTime){
motor[armMotor] = 50;
}//end while
state = inCorner;
break;
case findWall:
while(!SensorValue(leftSwitch) && !SensorValue(rightSwitch)){
motor[leftMotor] = motorBackwards;
motor[rightMotor] = motorForwards;
}//end while
state = switchPressed;
break;
case switchPressed:
motor[leftMotor] = OFF;
motor[rightMotor] = OFF;
while(SensorValue(leftSwitch) && !(SensorValue(rightSwitch))){
motor[leftMotor] = OFF;
motor[rightMotor] = 40;
}//end while
while(SensorValue(rightSwitch) && !(SensorValue(leftSwitch))){
motor[leftMotor] = -40;
motor[rightMotor] = OFF;
}//end while
if(SensorValue(sonarSensor) <= maxWallDistance && SensorValue(leftSwitch) && SensorValue(rightSwitch)){
state = armDown;
}else if(SensorValue(leftSwitch) && SensorValue(rightSwitch)){
wait1Msec(100);
if(SensorValue(sonarSensor) > 5){
state =inCorner;
}else{
state = armDown;
}//end if else
}//end if else
break;
case inCorner:
resetMotorEncoder(leftMotor);
while(getMotorEncoder(leftMotor) < 500){
motor[leftMotor] = motorForwards;
motor[rightMotor] = motorBackwards;
}//end while
resetMotorEncoder(leftMotor);
while(getMotorEncoder(leftMotor) > -250){
motor[leftMotor] = motorBackwards;
motor[rightMotor] = motorBackwards;
}//end while
motor[leftMotor] = OFF;
motor[rightMotor] = OFF;
state = findWall;
break;
case armDown:
motor[leftMotor] = OFF;
motor[rightMotor] = OFF;
clearTimer(T1);
while(time1[T1] < armMovementTime){
motor[armMotor] = - 40;
}//end while
state = signalCompletion;
break;
case signalCompletion:
clearTimer(T1);
while(time1[T1] < 2000){
motor[leftMotor] = motorForwards;
motor[rightMotor] = motorBackwards;
}//end while
while(time1[T1] < 3000){
motor[leftMotor] = -100;
motor[rightMotor] = -100;
}//end while
state = stopped;
break;
}//end switch
}//end while
}// end main
task usercontrol()
{
pidReset(flywheel);
debugStreamClear;
float sp = 2050;
// User control code here, inside the loop
int currentFlywheelSpeed;
// pidInit(flywheel, 0.10, 0.01, 0.00001, 3, 50);
pidInit(flywheel, 0.01, 0.03, 0.001, 3, 50);
unsigned long lastTime = nPgmTime;
resetMotorEncoder(flywheelLeftBot);
float rpm, lastRpm1, lastRpm2, lastRpm3, lastRpm4;
int counter = 0;
bool btn7DPressed;
while (true)
{
if(vexRT[btn7D] == 1)
{
btn7DPressed = true;
}
if(btn7DPressed == true)
{
dT = (float)(nPgmTime - lastTime)/1000;
lastTime = nPgmTime;
if(dT != 0)
{
rpm = 60.00*25*(((float)getMotorEncoder(flywheelLeftBot))/dT)/360;
}
else
{
rpm = 0;
}
resetMotorEncoder(flywheelLeftBot);
lastRpm4 = lastRpm3;
lastRpm3 = lastRpm2;
lastRpm2 = lastRpm1;
lastRpm1 = rpm;
rpmAvg = (rpm + lastRpm1 + lastRpm2 + lastRpm3 + lastRpm4) / 5;
if(flywheel.errorSum > 18000)
{
flywheel.errorSum = 18000;
}
if(rpmAvg >= sp && !set)
{
set = true;
}
if( rpm <= sp - 700)
{
set = false;
}
out = pidExecute(flywheel, sp-rpmAvg);
if(vexRT(Btn7U) == 1)
{
btn7DPressed = false;
pidReset(flywheel);
out = 0;
}
if(out < 1)
{
out = 1;
}
driveFlywheel(out);
}
/*
//motor[Motor1] = 127;
motor[flywheelLeftTop] = 68;
motor[flywheelLeftBot] = 68;
motor[flywheelRightTop] = 68;
motor[flywheelRightBot] = 68;
currentFlywheelSpeed = 68;
}
if(vexRT[Btn7L] == 1)
{
currentFlywheelSpeed -= 2;
motor[flywheelLeftTop] = currentFlywheelSpeed;
motor[flywheelLeftBot] = currentFlywheelSpeed;
motor[flywheelRightTop] = currentFlywheelSpeed;
motor[flywheelRightBot] = currentFlywheelSpeed;
wait1Msec(400);
}
if(vexRT[Btn7R] == 1)
{
currentFlywheelSpeed += 2;
motor[flywheelLeftTop] = currentFlywheelSpeed;
motor[flywheelLeftBot] = currentFlywheelSpeed;
motor[flywheelRightTop] = currentFlywheelSpeed;
motor[flywheelRightBot] = currentFlywheelSpeed;
wait1Msec(400);
}
if(vexRT[Btn7U] == 1)
{
// motor[Motor1] = 0;
motor[flywheelLeftTop] = 0;
motor[flywheelLeftBot] = 0;
motor[flywheelRightTop] = 0;
motor[flywheelRightBot] = 0;
}
if(vexRT[Btn8L] == 1)
{
motor[flywheelLeftTop] = 48;
motor[flywheelLeftBot] = 48;
motor[flywheelRightTop] = 48;
motor[flywheelRightBot] = 48;
currentFlywheelSpeed = 48;
}*/
if(vexRT[Btn8D] == 1)
{
motor[conveyor] = 127;
motor[conveyor2] = 127;
}
if(vexRT[Btn8R] == 1)
{
motor[conveyor] = 0;
motor[conveyor2] = 0;
}
if(vexRT[Btn8U] == 1)
{
motor[conveyor] = -127;
motor[conveyor2] = -127;
}
/* motor[driveFrontRight] = vexRT[Ch1] + vexRT[Ch4];
motor[driveFrontLeft] = vexRT[Ch1] - vexRT[Ch4];
motor[driveBackRight] = vexRT[Ch1] - vexRT[Ch4];
motor[driveBackLeft] = vexRT[Ch1] + vexRT[Ch4];*/
motor[driveFrontRight] = vexRT[Ch2];
motor[driveBackRight] = vexRT[Ch2];
motor[driveFrontLeft] = vexRT[Ch3];
motor[driveBackLeft] = vexRT[Ch3];
while(vexRT(Btn5U)) //Left strafe
{
motor[driveFrontRight] = 127;
motor[driveBackRight] = -127;
motor[driveFrontLeft] = -127;
motor[driveBackLeft] = 127;
}
while(vexRT(Btn6U))//RIGHT STRAFE
{
motor[driveFrontRight] = -127;
motor[driveBackRight] = 127;
motor[driveFrontLeft] = 127;
motor[driveBackLeft] = -127;
}
/*
if(vexRT[Ch2] > 2)
{
motor[driveFrontRight] = 127;
motor[driveFrontLeft] = -127;
motor[driveBackRight] = 127;
motor[driveBackLeft] = -127;
}
if(vexRT[Ch2] < -2)
{
motor[driveFrontRight] = -127;
motor[driveFrontLeft] = 127;
motor[driveBackRight] = -127;
motor[driveBackLeft] = 127;
}
*/
writeDebugStreamLine("%f", rpm);
wait1Msec(20);
}
}
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);
}
task autonomous()
{
clearTimer(T1);
pidInit(flywheel, 0.01, 0.03, 0.001, 3, 50);//p, i, d, epsilon, slew
unsigned long lastTime = nPgmTime;
resetMotorEncoder(flywheelLeftBot);
float rpm, lastRpm1, lastRpm2, lastRpm3, lastRpm4;
int counter = 0;
bool btn7DPressed;
while (true)
{
if(1==1)
{
dT = (float)(nPgmTime - lastTime)/1000;
lastTime = nPgmTime;
if(dT != 0)
{
rpm = 60.00*25*(((float)getMotorEncoder(flywheelLeftBot))/dT)/360;
}
else
{
rpm = 0;
}
resetMotorEncoder(flywheelLeftBot);
lastRpm4 = lastRpm3;
lastRpm3 = lastRpm2;
lastRpm2 = lastRpm1;
lastRpm1 = rpm;
rpmAvg = (rpm + lastRpm1 + lastRpm2 + lastRpm3 + lastRpm4) / 5;
if(flywheel.errorSum > 18000)
{
flywheel.errorSum = 18000;
}
if(rpmAvg >= sp && !set)
{
set = true;
}
if( rpm <= sp - 700)
{
set = false;
}
out = pidExecute(flywheel, sp-rpmAvg);
if(vexRT(Btn7U) == 1)
{
btn7DPressed = false;
pidReset(flywheel);
out = 0;
}
if(out < 1)
{
out = 1;
}
driveFlywheel(out);
}
// debugStrea
if(time1[T1] >= 7250 && time1[T1] <= 9000)
{
motor[conveyor] = 127;
motor[conveyor2] = 127;
}
}
/*dank memes
*/
/*
motor[flywheelLeftTop] = 63;
motor[flywheelLeftBot] = 63;
motor[flywheelRightTop] = 63;
motor[flywheelRightBot] = 63;
wait1Msec(2500);
motor[conveyor] = 127;
wait1Msec(1000);
motor[conveyor] = 0;
wait1Msec(1500);
motor[conveyor] = 127;
wait1Msec(1000);
motor[conveyor] = 0;
wait1Msec(1000);
motor[flywheelLeftTop] = 65;
motor[flywheelLeftBot] = 65;
motor[flywheelRightTop] = 65;
motor[flywheelRightBot] = 65;
motor[conveyor] = 127;
wait1Msec(1000);
motor[conveyor] = 0;
wait1Msec(1000);
motor[conveyor] = 127;
wait1Msec(1000);
motor[conveyor] = 0;
*/
//PROGRAMMING SKILLS
/* while(1==1)
{
motor[flywheelLeftTop] = 63;
motor[flywheelLeftBot] = 63;
motor[flywheelRightTop] = 63;
motor[flywheelRightBot] = 63;
motor[conveyor] = 127;
}*/
}
float calcDistMoved() {
return ((PI*WHEELDIAMETER)/ENCODER_COUNT_REVOLUTION) * (getMotorEncoder(LeftMotor)+getMotorEncoder(RightMotor))/2;
}
task main()
{
calibrateTiles();
pivotLeft(180, 30);
moveForward(180,30);
moveForward(30);
while(tileColor() != 1){ //REMAIN IN LOOP UNTIL BLACK REACHED
sleep(50);
}
while(tileColor() == 1){ //REMAIN IN LOOP WHILST ON BLACK
sleep(50);
}
stopMotors(); //STOP MOTORS WHEN GROUT REACHED
moveForward(30);
while(tileColor() == 1){
sleep(10);
}
stopMotors();
int blackwidth = getMotorEncoder(motorC); //$%^&*( CHECK THIS IS NOT A PROBLEM WITH THE ENCODER WIPING PART OF MOVEMETHODS
moveForward(blackwidth/2, 30);
sleep(500);
pivotRight(180, 30);
playSound(soundBeepBeep);
int tileTotal = 28;
int tileCount = 0;
prevColor = 1; //ITS ON BLACK at this point
while(tileCount < tileTotal){
switch(prevColor)
{
case 1:
while(tileColor() == 1){
sleep(50);
}
break;
case 2:
while(tileColor() == 2){
sleep (50);
}
break;
case 3:
//error correction begins
sleep(100);
if(tileColor() == 3){ //check that we're truly on grey
pivotLeft(10);
while(tileColor() == 3){
sleep(10);
}
stopMotors();
leftCorrectDist = getMotorEncoder(motorC);
pivotRight(leftCorrectDist);
if(tileColor() == 3){
pivotRight(10);
while(tileColor() == 3){
sleep(10);
}
}
rightCorrectDist = getMotorEncoder(motorB);
displayTextLine(6, "ERROR CORRECTION DISTANCES", black );
displayTextLine(7, "LEFT : %d", leftCorrectDist );
displayTextLine(8, "RIGHT : %d", rightCorrectDist );
sleep(500);
if(leftCorrectDist < rightCorrectDist){
pivotLeft(rightCorrectDist);
pivotLeft(leftCorrectDist);
displayTextLine(9, "ERROR CORRECTED WENT LEFT" );
}else{
sleep(50);
displayTextLine(9, "ERROR CORRECTED WENT RIGHT" );
}
//error correction ends
break;
default:
displayTextLine(6, "Unexpected error at switch 1.1");
break;
}
if(tileColor() == 1){
switch(prevColor) //begin switch 2.1
{
case 1:
sleep(20);
break;
case 2:
prevColor = 1;
tileCount++;
playSound(soundBlip);
displayTextLine(6, "Tiles Counted : %d", tileCount );
break;
case 3:
prevColor = 1;
displayTextLine(5, "Error corrected 'back to black'");
break;
default:
displayTextLine(6, "Unexpected error at switch 2.1");
break;
} //End of Switch 2.1
}else{
if(tileColor() == 2){
switch(prevColor){ //begin switch 2.2
case 1:
prevColor = 2;
tileCount++;
playSound(soundBlip);
displayTextLine(6, "Tiles Counted : %d", tileCount );
break;
case 2:
sleep(20);
break;
case 3:
prevColor = 2;
displayTextLine(5, "Error corrected 'back from the white'");
break;
default:
displayTextLine(6, "Unexpected error at switch 2.2");
break;
}//end of switch 2.2
}else{
switch(prevColor){ //begin switch 2.3
case 1:
lostTile = 1;
prevColor = 3;
break;
case 2:
lostTile = 2;
prevColor = 3;
break;
case 3:
displayTextLine(10, "HELP WE'RE STILL LOST");
break;
default:
displayTextLine(6, "Unexpected error at switch 2.3");
break;
}// end of switch 2.3
}
}
}
}
finalMethod();
}
task main() {
int leftMotorSpeed, rightMotorSpeed, wristMotorSpeed, armMotorSpeed, stickAValue, stickBValue, stickCValue, stickDValue;
resetMotorEncoder(armMotor);
resetMotorEncoder(clawMotor);
resetMotorEncoder(leftMotor);
resetMotorEncoder(rightMotor);
resetMotorEncoder(wristMotor);
resetGyro(gyro);
startGyroCalibration(gyro, gyroCalibrateSamples2048);
eraseDisplay();
getGyroCalibrationFlag(gyro);
displayString(3, "calibrating gyro");
wait1Msec(500);
eraseDisplay();
startTask(displayMyMotorPositions);
while (true ){
/***************************************************************************************************************/
//Joystick Control:
/***************************************************************************************************************/
//Driving:
if(getJoystickValue(BtnEUp) > 0) { //Drive Straight Forward
driveStraightForward();
}
else if(getJoystickValue(BtnEDown) > 0) { //Drive Straight Backward
driveStraightBackward();
}
else {
driveForwardPressed = false;
driveBackwardPressed = false;
stickAValue = getJoystickValue(ChA);
if ((stickAValue <= 15) && (stickAValue >= -15) ) stickAValue = 0;
stickBValue = getJoystickValue(ChB);
if ((stickBValue <= 15) && (stickBValue >= -15) ) stickBValue = 0;
if (stickBValue >=0 ) stickBValue = (stickBValue / 10) * (stickBValue / 10) * 2;
else stickBValue = - (stickBValue / 10) * (stickBValue / 10) * 2;
leftMotorSpeed = stickAValue - stickBValue;
rightMotorSpeed = stickAValue + stickBValue;
if ( leftMotorSpeed > 100) leftMotorSpeed = 100;
if ( leftMotorSpeed < -100) leftMotorSpeed = -100;
if ( rightMotorSpeed > 100) rightMotorSpeed = 100;
if ( rightMotorSpeed < -100) rightMotorSpeed = -100;
if (leftMotorSpeed >=0 ) leftMotorSpeed = (leftMotorSpeed / 10) * (leftMotorSpeed / 10);
else leftMotorSpeed = - (leftMotorSpeed / 10) * (leftMotorSpeed / 10);
if (rightMotorSpeed >=0 ) rightMotorSpeed = (rightMotorSpeed / 10) * (rightMotorSpeed / 10);
else rightMotorSpeed = - (rightMotorSpeed / 10) * (rightMotorSpeed / 10);
setMotorSpeed(leftMotor, leftMotorSpeed);
setMotorSpeed(rightMotor, rightMotorSpeed);
}
/***************************************************************************************************************/
//WRIST MOTOR
stickDValue = getJoystickValue(ChD);
if ((stickDValue <= 15) && (stickDValue >= -15) ) stickDValue = 0;
wristMotorSpeed = stickDValue;
if (wristMotorSpeed >=0 ) wristMotorSpeed = (wristMotorSpeed / 10) * (wristMotorSpeed / 10);
else wristMotorSpeed = - (wristMotorSpeed / 10) * (wristMotorSpeed / 10);
globalWristPosition = getMotorEncoder(wristMotor);
if ((wristMotorSpeed > 0) && (globalWristPosition >= WRIST_UPPER_LIMIT)){
setMotorTarget(wristMotor, WRIST_UPPER_LIMIT, 70);
}
else if ((wristMotorSpeed < 0) && (globalWristPosition <= WRIST_LOWER_LIMIT)) {
setMotorTarget(wristMotor, WRIST_LOWER_LIMIT, 70);
}
else {
//slow things down if we are near the limit of wrist movement
if ( abs(globalWristPosition - WRIST_LOWER_LIMIT) < 10) {
wristMotorSpeed = wristMotorSpeed / 4;
}
if ( abs(globalWristPosition - WRIST_UPPER_LIMIT) < 10) {
wristMotorSpeed = wristMotorSpeed / 4;
}
setMotorSpeed(wristMotor, wristMotorSpeed );
}
/***************************************************************************************************************/
//ARM MOTOR
stickCValue = getJoystickValue(ChC);
if ((stickCValue <= 15) && (stickCValue >= -15) ) stickCValue = 0;
armMotorSpeed = stickCValue;
if (armMotorSpeed >=0 ) armMotorSpeed = (armMotorSpeed / 10) * (armMotorSpeed / 10);
else armMotorSpeed = - (armMotorSpeed / 10) * (armMotorSpeed / 10);
globalArmPosition = getMotorEncoder(armMotor);
if ((armMotorSpeed > 0) && (globalArmPosition >= ARM_UPPER_LIMIT)){
setMotorTarget(armMotor, ARM_UPPER_LIMIT, 70);
}
else if ((armMotorSpeed < 0) && (globalArmPosition <= ARM_LOWER_LIMIT)) {
setMotorTarget(armMotor, ARM_LOWER_LIMIT, 70);
}
else {
//slow things down if we are near the limit of arm movement
if ( abs(globalArmPosition - ARM_LOWER_LIMIT) < 10){
armMotorSpeed= armMotorSpeed/ 4;
}
if ( abs(globalArmPosition - ARM_UPPER_LIMIT) < 10){
armMotorSpeed= armMotorSpeed/ 4;
}
setMotorSpeed(armMotor, armMotorSpeed);
}
/***************************************************************************************************************/
//CLAW MOTOR
if(getJoystickValue(BtnLUp) > 0) { //CLOSE
setMotorSpeed(clawMotor, 70);
}
else if(getJoystickValue(BtnLDown) > 0) { //OPEN
globalClawPosition = getMotorEncoder(clawMotor);
if (globalClawPosition <= -87) {
setMotorTarget(clawMotor, -87, 70);
}
else {
setMotorSpeed(clawMotor, -70);
}
}
else {
globalClawPosition = getMotorEncoder(clawMotor);
setMotorTarget(clawMotor, globalClawPosition, 90);
}
/***************************************************************************************************************/
//Buttons to move our Arm for Us quickly to other positions
if(getJoystickValue(BtnFUp) > 0) { //UP
moveToAimingPosition();
}
if(getJoystickValue(BtnFDown) > 0) { //GRAB a Block
moveToNeutralPosition();
}
/***************************************************************************************************************/
//Block Stacking Positions
//Buttons to move our Arm for Us quickly to other positions
if(getJoystickValue(BtnRUp) > 0) { //Upper Stacking Position
moveToUpperStackingPosition();
}
if(getJoystickValue(BtnRDown) > 0) { //Lower Stacking Position
moveToLowerStackingPosition();
}
wait1Msec(50); // Give actions time to make progress and prevent over-control from taking inputs too fast
}
}