//drive forward using the motor encoders use sleepTime to wait until the motors
//stop
void driveForwardWithEncoder(int count, int speed, int sleepTime) {
resetMotorEncoder(LEFT_DRIVETRAIN_MOTOR);
resetMotorEncoder(RIGHT_DRIVETRAIN_MOTOR);
setMotorTarget(LEFT_DRIVETRAIN_MOTOR, count, speed);
setMotorTarget(RIGHT_DRIVETRAIN_MOTOR, -count, -speed);
sleep(sleepTime);
}
void pivotRight(int distance, int speed){
resetMotorEncoder(motorB);
resetMotorEncoder(motorC);
setMotorTarget(motorB, distance, speed);
setMotorTarget(motorC, distance, -speed);
sleep(100 * distance / speed);
}
void turnLeft(int distance, int speed){
resetMotorEncoder(motorB);
resetMotorEncoder(motorC);
setMotorSpeed(motorB, 0);
setMotorTarget(motorC, distance, speed);
sleep(100 * distance / speed);
}
void moveForward(int distance, int speed){
resetMotorEncoder(motorB);
resetMotorEncoder(motorC);
setMotorTarget(motorB, distance, speed);
setMotorTarget(motorC, distance, speed);
sleep(100 * distance / speed);
}
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);
}
}
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 initialize(){
slaveMotor(backRight, frontRight);
slaveMotor(backLeft, frontLeft);
slaveMotor(liftRight, liftLeft);
resetMotorEncoder(liftLeft);
resetMotorEncoder(backLeft);
resetMotorEncoder(claw);
wait1Msec(100);
}
void resetIME ()
{
resetMotorEncoder(right);
resetMotorEncoder(left);
resetMotorEncoder(grabL);
resetMotorEncoder(armR3);
resetMotorEncoder(armL3);
clearTimer(T1);
}
/// drive the robot forward using motor encoders
void driveWithEncoders(int degrees){
resetMotorEncoder(leftMotor);
resetMotorEncoder(rightMotor);
setMotorTarget(leftMotor, degrees, DRIVE_SPEED);
setMotorTarget(rightMotor, degrees, DRIVE_SPEED);
waitUntilMotorStop(leftMotor);
setMotorSpeed(leftMotor, 0);
setMotorSpeed(rightMotor, 0);
sleep(200);
}
/// drive a certain distance in inches
void driveInches(float inches, int driveSpeed) {
resetMotorEncoder(leftMotor);
resetMotorEncoder(rightMotor);
setMotorTarget(leftMotor, inches/7.8, driveSpeed);
setMotorTarget(rightMotor, inches/7.8, driveSpeed);
waitUntilMotorStop(leftMotor);
setMotorSpeed(leftMotor, 0);
setMotorSpeed(rightMotor, 0);
sleep(200);
}
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);
}
void side(int degrees) {
setMotorSpeed(RWheel, 8);
setMotorSpeed(LWheel, 8);
resetMotorEncoder(motorA);
resetMotorEncoder(motorC);
int x = getMotorEncoder(motorA);
while (x < degrees) {
x = getMotorEncoder(motorA)
}
}
void turnClockWise(){
resetMotorEncoder( motorA );
resetMotorEncoder( motorB );
setMotorTarget( motorA, ANGLE, VELOCITY );
setMotorTarget( motorB, ANGLE, -VELOCITY );
delay(390);
while( SensorValue[COLOR] != BLACK){};
setMotorA(0);
setMotorB(0);
current_direction = clockWiseDirection( current_direction );
}
//Controls all movement tasks
void motor_execute(motor_control* motor_tasks,byte* head_motor,byte* start_motor,system_state* current_state)
{
if ((*start_motor) <= 0) //Completed task
{
if ((((getMotorTargetCompleted(motor1)) && (getMotorTargetCompleted(motor2))) || (time1[T1] > FAILOVER_TIME)) || ((*start_motor) == -1))
{
//Task completed
resetMotorEncoder(motor1); //Stop motor
motor[motor1] = 0;
moveMotorTarget(motor1, 0, 0, 0);
resetMotorEncoder(motor2);
motor[motor2] = 0;
moveMotorTarget(motor2, 0, 0, 0);
byte i;
*current_state = motor_tasks[0].next_state;
for (i=0; i< (*head_motor) ;++i) //Delete first task
{
motor_tasks[i].motor1_power = motor_tasks[i+1].motor1_power;
motor_tasks[i].motor1_encoder = motor_tasks[i+1].motor1_encoder;
motor_tasks[i].motor2_power = motor_tasks[i+1].motor2_power;
motor_tasks[i].motor2_encoder = motor_tasks[i+1].motor2_encoder;
motor_tasks[i].next_state = motor_tasks[i+1].next_state;
}
if ((--(*head_motor)) == 0)
--(*head_motor);
else
*start_motor = 1;
}
#if DEBUG
writeDebugStreamLine("Task ended");
#endif
}
else if ((*start_motor) == 1) //Start moving
{
#if DEBUG
writeDebugStreamLine("Staring task");
#endif
clearTimer(T1); //Failover timer
resetMotorEncoder(motor1);
moveMotorTarget(motor1, motor_tasks[0].motor1_encoder, motor_tasks[0].motor1_power, 0);
resetMotorEncoder(motor2);
moveMotorTarget(motor2, motor_tasks[0].motor2_encoder, motor_tasks[0].motor2_power, 0);
*start_motor = 0;
}
}
task Pid2() {
resetMotorEncoder(leftLauncher);
while (true) {
desiredSpeed = launcherSpeed;
currentPos = nMotorEncoder[leftLauncher];
actualSpeed = (currentPos - prevPos);
speedError = (desiredSpeed - actualSpeed);
intError += speedError;
difError = prevError - speedError;
// motor speed is 75 when motorSpeed = 127
motorSpeed = (speedError * kp / kpden) + (intError * ki / kiden) + (difError * kd / kdden);
//motorSpeed = 127;
prevPos = currentPos;
prevError = speedError;
if (motorSpeed > 127) {
motorSpeed = 127;
}
if (motorSpeed < 0) {
motorSpeed = 0;
}
motor[leftLauncher] = motorSpeed;
}
}
/// move arm back to bottom
void homeArm() {
while(!limitSwitchPressed()){
setMotorSpeed(armMotor, -ARM_SPEED);
}
setMotorSpeed(armMotor, 0);
resetMotorEncoder(armMotor);
}
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 readRPM() {
motor[testMotor] = 127;
while(true) {
resetMotorEncoder(testMotor);
wait1Msec(2500);
rpm = (nMotorEncoder[testMotor]/360.0) * 24.0;
}
}
/// 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()
{
// Variables
long encnow = 0;
float rpmconversion = 21*((60.0)/392.0);
// Reset the encoder value to zero
resetMotorEncoder(InsideMotor);
SensorValue[I2C_1] = 0;
// Clear the debug window
clearDebugStream();
while(true)
{
// Iterate through the different power levels 0 to -128
for(int i = -1; i >= -128; i--)
{
// Set the motor powers
motor[InsideMotor] = i;
motor[OutsideMotor] = i;
// Wait one second and then get the encoder value
wait(1, seconds);
encnow = SensorValue[I2C_1];
// Read the encoder value and output the sensor value and rpm calculation
writeDebugStreamLine("%d\t%d\t%f", i, encnow, (rpmconversion*encnow));
// Reset the ticks of the sensor
SensorValue[I2C_1] = 0;
if(i == -128)
{
for(int x = -128; x < 0; x = x + 20)
{
motor[InsideMotor] = x;
motor[OutsideMotor] = x;
wait(500,milliseconds);
}
motor[InsideMotor] = 0;
motor[OutsideMotor] = 0;
wait(1,seconds);
SensorValue[I2C_1] = 0;
}
}
// Iterate through the different power levels, 0 to 127
for(int i = 0; i <= 127; i++)
{
// Set the motor powers
motor[InsideMotor] = i;
motor[OutsideMotor] = i;
// Wait one second and then get the encoder value
wait(1, seconds);
encnow = SensorValue[I2C_1];
// Read the encoder value and output the sensor value and rpm calculation
writeDebugStreamLine("%d\t%d\t%f", i, encnow, (rpmconversion*encnow));
// Reset the ticks of the sensor
SensorValue[I2C_1] = 0;
if(i == 127)
{
for(int x = 0; x > 0; x = x - 20)
{
motor[InsideMotor] = x;
motor[OutsideMotor] = x;
wait(500,milliseconds);
}
motor[InsideMotor] = 0;
motor[OutsideMotor] = 0;
wait(1,seconds);
SensorValue[I2C_1] = 0;
}
}
}
}
void pivotRight(int speed){
resetMotorEncoder(motorB);
resetMotorEncoder(motorC);
setMotorSpeed(motorB, speed);
setMotorSpeed(motorC, -speed);
}
void turnRight(int speed){
resetMotorEncoder(motorB);
resetMotorEncoder(motorC);
setMotorSpeed(motorB, speed);
setMotorSpeed(motorC, 0);
}
/// move arm to certain degrees
void setArmDegrees(float degrees) {
resetMotorEncoder(armMotor);
setMotorTarget(armMotor, degrees/360, ARM_SPEED);
waitUntilMotorStop(armMotor);
}
void moveForward(int speed){
resetMotorEncoder(motorB);
resetMotorEncoder(motorC);
setMotorSpeed(motorB, speed);
setMotorSpeed(motorC, speed);
}
//rotate the lift motor a certain amount
void lift(int rotatos) {
resetMotorEncoder(LIFT_MOTOR);
setServoTarget(LIFT_MOTOR, rotatos);
sleep(1000);
}
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
}
}
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 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;
}*/
}
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
void turnLeft(int speed){
resetMotorEncoder(motorB);
resetMotorEncoder(motorC);
setMotorSpeed(motorB, 0);
setMotorSpeed(motorC, speed);
}
