Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #pragma config(I2C_Usage, I2C1, i2cSensors)
- #pragma config(Sensor, in1, gyro, sensorGyro)
- #pragma config(Sensor, in2, potentiometerLt, sensorPotentiometer)
- #pragma config(Sensor, in3, potentiometerRt, sensorPotentiometer)
- #pragma config(Sensor, dgtl1, claw, sensorDigitalOut)
- #pragma config(Sensor, dgtl3, platform, sensorDigitalOut)
- #pragma config(Sensor, I2C_1, , sensorQuadEncoderOnI2CPort, , AutoAssign)
- #pragma config(Sensor, I2C_2, , sensorQuadEncoderOnI2CPort, , AutoAssign)
- #pragma config(Motor, port1, Intake1, tmotorVex393_HBridge, openLoop, reversed)
- #pragma config(Motor, port2, RtBackWheel, tmotorVex393HighSpeed_MC29, openLoop, reversed, encoderPort, I2C_2)
- #pragma config(Motor, port3, LtDrive, tmotorVex393HighSpeed_MC29, openLoop)
- #pragma config(Motor, port4, RtBackLift, tmotorVex393HighSpeed_MC29, openLoop, reversed)
- #pragma config(Motor, port5, LtBackLift, tmotorVex393HighSpeed_MC29, openLoop)
- #pragma config(Motor, port6, LtBackWheel, tmotorVex393HighSpeed_MC29, openLoop, encoderPort, I2C_1)
- #pragma config(Motor, port7, RtDrive, tmotorVex393HighSpeed_MC29, openLoop, reversed)
- #pragma config(Motor, port8, LtFrontLift, tmotorVex393HighSpeed_MC29, openLoop)
- #pragma config(Motor, port9, RtFrontLift, tmotorVex393HighSpeed_MC29, openLoop, reversed)
- #pragma config(Motor, port10, Intake2, tmotorVex393_HBridge, openLoop)
- //*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
- #pragma platform(VEX)
- //Competition Control and Duration Settings
- #pragma competitionControl(Competition)
- #pragma autonomousDuration(20)
- #pragma userControlDuration(120)
- #include "Vex_Competition_Includes.c" //Main competition background code...do not modify!
- #define LEFT 1
- #define RIGHT -1
- int buttonToggleState1 = 0;
- int buttonPressed1 = 0;
- int buttonToggleState2 = 0;
- int buttonPressed2 = 0;
- int buttonPressed3 = 0;
- int buttonPressed4 = 0;
- bool RightLower = true;
- bool leftHigher = true;
- const unsigned int TrueSpeed[128] =
- {
- 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
- 0, 21, 21, 21, 22, 22, 22, 23, 24, 24,
- 25, 25, 25, 25, 26, 27, 27, 28, 28, 28,
- 28, 29, 30, 30, 30, 31, 31, 32, 32, 32,
- 33, 33, 34, 34, 35, 35, 35, 36, 36, 37,
- 37, 37, 37, 38, 38, 39, 39, 39, 40, 40,
- 41, 41, 42, 42, 43, 44, 44, 45, 45, 46,
- 46, 47, 47, 48, 48, 49, 50, 50, 51, 52,
- 52, 53, 54, 55, 56, 57, 57, 58, 59, 60,
- 61, 62, 63, 64, 65, 66, 67, 67, 68, 70,
- 71, 72, 72, 73, 74, 76, 77, 78, 79, 79,
- 80, 81, 83, 84, 84, 86, 86, 87, 87, 88,
- 88, 89, 89, 90, 90,127,127,127
- };
- /////////////////////////////////////////////////////////////////////////////////////////
- //
- // Pre-Autonomous Functions
- //
- // You may want to perform some actions before the competition starts. Do them in the
- // following function.
- //
- /////////////////////////////////////////////////////////////////////////////////////////
- void fwds(int distance, int speed)
- {
- nMotorEncoder[RtBackWheel] = 0;
- nMotorEncoder[LtBackWheel] = 0;
- while(nMotorEncoder[RtBackWheel] < distance && nMotorEncoder[LtBackWheel] > (distance * -1))
- {
- motor[LtDrive] = speed;
- motor[LtBackWheel] = speed;
- motor[RtDrive] = speed;
- motor[RtBackWheel] = speed;
- }
- motor[LtDrive] = 0;
- motor[LtBackWheel] = 0;
- motor[RtDrive] = 0;
- motor[RtBackWheel] = 0;
- }
- void bwds(int distance, int speed)
- {
- fwds(distance, -speed);
- }
- void turn(int angle, int speed, int direction)
- {
- int gError = SensorValue[gyro];
- while(direction * (SensorValue[gyro] - gError) < angle)
- {
- motor[LtBackWheel] = -direction * speed;
- motor[LtDrive] = -direction * speed;
- motor[RtBackWheel] = direction * speed;
- motor[RtDrive] = direction * speed;
- }
- motor[LtBackWheel] = 0;
- motor[LtDrive] = 0;
- motor[RtBackWheel] = 0;
- motor[RtDrive] = 0;
- }
- void swing(int angle, int speed, int direction)
- {
- if(direction > 0)
- {
- while(direction * SensorValue[in1] < angle)
- {
- motor[LtBackWheel] = 0;
- motor[LtDrive] = 0;
- motor[RtBackWheel] = direction * speed;
- motor[RtDrive] = direction * speed;
- }
- }
- else
- {
- while(direction * SensorValue[in1] < angle)
- {
- motor[LtBackWheel] = -direction * speed;
- motor[LtDrive] = -direction * speed;
- motor[RtBackWheel] = 0;
- motor[RtDrive] = 0;
- }
- }
- motor[LtBackWheel] = 0;
- motor[LtDrive] = 0;
- motor[RtBackWheel] = 0;
- motor[RtDrive] = 0;
- }
- void bwdslft(int distance, int speed)
- {
- nMotorEncoder[RtBackWheel] = 0;
- nMotorEncoder[LtBackWheel] = 0;
- while(nMotorEncoder[RtBackWheel] > (distance * -1) && nMotorEncoder[LtBackWheel] < distance)
- {
- motor[LtDrive] = -speed;
- motor[LtBackWheel] = -speed;
- motor[RtDrive] = -speed;
- motor[RtBackWheel] = -speed;
- motor[RtBackLift] = 127;
- motor[RtFrontLift] = 127;
- motor[LtBackLift] = 127;
- motor[LtFrontLift] = 127;
- }
- motor[LtDrive] = 0;
- motor[LtBackWheel] = 0;
- motor[RtDrive] = 0;
- motor[RtBackWheel] = 0;
- motor[RtBackLift] = 0;
- motor[RtFrontLift] = 0;
- motor[LtBackLift] = 0;
- motor[LtFrontLift] = 0;
- }
- void liftTime(int time)
- {
- motor[RtFrontLift] = 127;
- motor[LtFrontLift] = 127;
- motor[RtBackLift] = 127;
- motor[LtBackLift] = 127;
- wait1Msec(time);
- motor[RtFrontLift] = 0;
- motor[LtFrontLift] = 0;
- motor[RtBackLift] = 0;
- motor[LtBackLift] = 0;
- }
- void liftHold()
- {
- motor[RtFrontLift] = 10;
- motor[LtFrontLift] = 10;
- motor[RtBackLift] = 10;
- motor[LtBackLift] = 10;
- }
- void liftup(int distance, int speed)
- {
- while(SensorValue[potentiometerLt] > distance/* && SensorValue[potentiometerLt] > distance*/)
- {
- motor[LtFrontLift] = speed;
- motor[LtBackLift] = speed;
- motor[RtFrontLift] = speed;
- motor[RtBackLift] = speed;
- }
- motor[LtFrontLift] = 10;
- motor[LtBackLift] = 10;
- motor[RtFrontLift] = 10;
- motor[RtBackLift] = 10;
- }
- void liftdown(int distance, int speed)
- {
- while(SensorValue[potentiometerRt] < distance)
- {
- motor[LtFrontLift] = -speed;
- motor[LtBackLift] = -speed;
- motor[RtFrontLift] = -speed;
- motor[RtBackLift] = -speed;
- }
- motor[LtFrontLift] = 0;
- motor[LtBackLift] = 0;
- motor[RtFrontLift] = 0;
- motor[RtFrontLift] = 0;
- }
- void intake(int time, int speed)
- {
- motor[Intake1] = -speed;
- motor[Intake2] = -speed;
- wait1Msec(time);
- motor[Intake1] = 0;
- motor[Intake2] = 0;
- }
- void outtake(int time, int speed)
- {
- motor[Intake1] = speed;
- motor[Intake2] = speed;
- wait1Msec(time);
- motor[Intake1] = 0;
- motor[Intake2] = 0;
- }
- void skyriseintake()
- {
- SensorValue[claw] = 1;
- }
- void skyriseouttake()
- {
- SensorValue[claw] = 0;
- }
- void liftTick (int distance, int speed, int maxTicks)
- {
- int ticks = 0;
- while(ticks <= maxTicks)
- {
- if(SensorValue[potentiometerRt] > distance)
- {
- motor[LtFrontLift] = speed;
- motor[LtBackLift] = speed;
- motor[RtFrontLift] = speed;
- motor[RtBackLift] = speed;
- }
- else if(SensorValue[potentiometerRt] < (distance - 150))
- {
- motor[RtFrontLift] = -speed;
- motor[LtFrontLift] = -speed;
- motor[RtBackLift] = -speed;
- motor[LtBackLift] = -speed;
- }
- else
- {
- motor[LtFrontLift] = 10;
- motor[LtBackLift] = 10;
- motor[RtFrontLift] = 10;
- motor[RtBackLift] = 10;
- ticks++;
- }
- wait1Msec(25);
- }
- motor[LtFrontLift] = 0;
- motor[LtBackLift] = 0;
- motor[RtFrontLift] = 0;
- motor[RtBackLift] = 0;
- }
- void intakefwds(int distance, int speed)
- {
- nMotorEncoder[RtBackWheel] = 0;
- nMotorEncoder[LtBackWheel] = 0;
- while(nMotorEncoder[RtBackWheel] < distance && nMotorEncoder[LtBackWheel] > (distance * -1))
- {
- motor[LtDrive] = speed;
- motor[LtBackWheel] = speed;
- motor[RtDrive] = speed;
- motor[RtBackWheel] = speed;
- motor[Intake1] = -127;
- motor[Intake2] = -127;
- }
- motor[LtDrive] = 0;
- motor[LtBackWheel] = 0;
- motor[RtDrive] = 0;
- motor[RtBackWheel] = 0;
- motor[Intake1] = 0;
- motor[Intake2] = 0;
- }
- void setup()
- {
- SensorType[in1] = sensorNone;
- SensorType[in1] = sensorGyro;
- outtake(250,127);
- intake(250,127);
- outtake(250,127);
- intake(250,127);
- motor[RtFrontLift] = 127;
- motor[LtFrontLift] = 127;
- motor[RtBackLift] = 127;
- motor[LtBackLift] = 127;
- wait1Msec(300);
- motor[RtFrontLift] = -10;
- motor[LtFrontLift] = -10;
- motor[RtBackLift] = -10;
- motor[LtBackLift] = -10;
- wait1Msec(200);
- motor[RtFrontLift] = 127;
- motor[LtFrontLift] = 127;
- motor[RtBackLift] = 127;
- motor[LtBackLift] = 127;
- wait1Msec(400);
- liftdown(2500,127);
- }
- void pre_auton()
- {
- // Set bStopTasksBetweenModes to false if you want to keep user created tasks running between
- // Autonomous and Tele-Op modes. You will need to manage all user created tasks if set to false.
- bStopTasksBetweenModes = true;
- // All activities that occur before the competition starts
- // Example: clearing encoders, setting servo positions, ...
- }
- /////////////////////////////////////////////////////////////////////////////////////////
- //
- // Autonomous Task
- //
- // This task is used to control your robot during the autonomous phase of a VEX Competition.
- // You must modify the code to add your own robot specific commands here.
- //
- /////////////////////////////////////////////////////////////////////////////////////////
- task autonomous()
- {
- //function(distance/time, speed)
- //not actual auton
- setup();
- intakefwds(500,100);
- intake(500,127);
- turn(3000,100,RIGHT);
- }
- // .....................................................................................
- // Insert user code here.
- // .....................................................................................
- // AutonomousCodePlaceholderForTesting(); // Remove this function call once you have "real" code.
- /////////////////////////////////////////////////////////////////////////////////////////
- //
- // User Control Task
- //
- // This task is used to control your robot during the user control phase of a VEX Competition.
- // You must modify the code to add your own robot specific commands here.
- //
- /////////////////////////////////////////////////////////////////////////////////////////
- int error;
- float kP = 0.6 ;
- int offSet;
- //bool threshold;
- //bool toTheRight;
- //bool toTheLeft;
- task usercontrol()
- {
- // User control code here, inside the loop
- int RT1;
- int RT2;
- while (true)
- {
- if(vexRT[Btn8D] == 1)
- {
- //actual auton
- setup();
- intakefwds(500,40);
- intake(1500,127);
- fwds(800,100);
- bwds(100,100);
- swing(250,100,LEFT);
- bwds(500,100);
- turn(130,100,LEFT);
- bwds(200,100);
- liftup(1200,127);
- }
- //Lift adjust
- /*if(abs(SensorValue(potentiometerRt) - (SensorValue(potentiometerLt) + offSet)) > 200)
- {
- threshold = true;
- }
- else
- {
- threshold = false;
- }*/
- if(SensorValue(potentiometerLt) > 1860)
- {
- offSet = 40;
- }
- else if(SensorValue(potentiometerLt) > 1175)
- {
- offSet = 40;
- }
- else
- {
- offSet = 40;
- }
- error = abs(SensorValue(potentiometerRt) - (SensorValue(potentiometerLt) + offSet)) * kP;
- //Automagic
- if( vexRT[ Btn7U ] == 1 )
- {
- if( ! buttonPressed3)
- {
- if(SensorValue[potentiometerLt] > SensorValue[potentiometerRt])
- {
- leftHigher = false;
- }
- else
- {
- leftHigher = true;
- }
- buttonPressed3 = 1;
- }
- }
- else
- {
- buttonPressed3 = 0;
- }
- if( vexRT[ Btn7D ] == 1 )
- {
- if( ! buttonPressed4)
- {
- if((SensorValue[potentiometerLt] + 100) > SensorValue[potentiometerRt])
- {
- RightLower = true;
- }
- else
- {
- RightLower = false;
- }
- buttonPressed4 = 1;
- }
- }
- else
- {
- buttonPressed4 = 0;
- }
- //Base
- RT1 = vexRT[Ch2];
- RT2 = vexRT[Ch3];
- if(RT2 > 0)
- {
- motor[LtDrive] = TrueSpeed[RT2];
- motor[LtBackWheel] = TrueSpeed[RT2];
- }
- else if(RT2 < 0)
- {
- RT2 = RT2 * -1;
- motor[LtDrive] = -TrueSpeed[RT2];
- motor[LtBackWheel] = -TrueSpeed[RT2];
- }
- if(RT1 > 0)
- {
- motor[RtDrive] = TrueSpeed[RT1];
- motor[RtBackWheel] = TrueSpeed[RT1];
- }
- else if(RT1 < 0)
- {
- RT1 = RT1 * -1;
- motor[RtDrive] = -TrueSpeed[RT1];
- motor[RtBackWheel] = -TrueSpeed[RT1];
- }
- //Lift
- if(vexRT[Btn7D] == 1 && RightLower)//Automagic down
- {
- motor[RtFrontLift] = -50;
- motor[LtFrontLift] = 17;
- motor[RtBackLift] = -50;
- motor[LtBackLift] = 17;
- }
- else if(vexRT[Btn7D] == 1 && !RightLower)
- {
- motor[RtFrontLift] = 17;
- motor[LtFrontLift] = -50;
- motor[RtBackLift] = 17;
- motor[LtBackLift] = -50;
- }
- else if(vexRT[Btn7U] == 1 && !leftHigher)//Automagic up
- {
- motor[RtFrontLift] = 40;
- motor[LtFrontLift] = 80;
- motor[RtBackLift] = 40;
- motor[LtBackLift] = 80;
- }
- else if(vexRT[Btn7U] == 1 && leftHigher)
- {
- motor[RtFrontLift] = 80;
- motor[LtFrontLift] = 40;
- motor[RtBackLift] = 80;
- motor[LtBackLift] = 40;
- }
- else if(vexRT[Btn5D] == 1 && vexRT[Btn5U] == 1)//lifthold
- {
- motor[RtFrontLift] = 10;
- motor[LtFrontLift] = 10;
- motor[RtBackLift] = 10;
- motor[LtBackLift] = 10;
- }
- else if(vexRT[Btn8L] == 1)//lift adjust
- {
- if((SensorValue(potentiometerLt) + offSet) > SensorValue(potentiometerRt)/* && threshold*/)
- {
- motor[RtFrontLift] = 127 - error;
- motor[LtFrontLift] = 127;
- motor[RtBackLift] = 127 - error;
- motor[LtBackLift] = 127;
- }
- else if(SensorValue(potentiometerRt) > (SensorValue(potentiometerLt) + offSet)/* && threshold*/)
- {
- motor[RtFrontLift] = 127;
- motor[LtFrontLift] = 127 - error;
- motor[RtBackLift] = 127;
- motor[LtBackLift] = 127 - error;
- }
- else
- {
- motor[RtFrontLift] = 127;
- motor[LtFrontLift] = 127;
- motor[RtBackLift] = 127;
- motor[LtBackLift] = 127;
- }
- }
- else if(vexRT[Btn5D] == 1)//lift down
- {
- motor[RtFrontLift] = -127;
- motor[LtFrontLift] = -127;
- motor[RtBackLift] = -127;
- motor[LtBackLift] = -127;
- }
- else if(vexRT[Btn5U] == 1)//lift up
- {
- motor[RtFrontLift] = 127;
- motor[LtFrontLift] = 127;
- motor[RtBackLift] = 127;
- motor[LtBackLift] = 127;
- }
- else if(vexRT[Btn7L] == 1)//autotap
- {
- if(SensorValue(potentiometerRt) > 2300)
- {
- if(SensorValue(potentiometerRt) > 2450)
- {
- motor[RtFrontLift] = 30;
- motor[LtFrontLift] = 30;
- motor[RtBackLift] = 30;
- motor[LtBackLift] = 30;
- }
- else
- {
- motor[RtFrontLift] = 10;
- motor[LtFrontLift] = 10;
- motor[RtBackLift] = 10;
- motor[LtBackLift] = 10;
- }
- }
- else
- {
- if(SensorValue(potentiometerRt) < 2300)
- {
- motor[RtFrontLift] = -127;
- motor[LtFrontLift] = -127;
- motor[RtBackLift] = -127;
- motor[LtBackLift] = -127;
- }
- else
- {
- motor[RtFrontLift] = 10;
- motor[LtFrontLift] = 10;
- motor[RtBackLift] = 10;
- motor[LtBackLift] = 10;
- }
- }
- }
- else
- {
- motor[RtFrontLift] = 0;
- motor[LtFrontLift] = 0;
- motor[RtBackLift] = 0;
- motor[LtBackLift] = 0;
- }
- //Intake
- if(vexRT[Btn6D] == 1)
- {
- motor[Intake1] = 127;
- motor[Intake2] = 127;
- }
- else if (vexRT[Btn6U] == 1)
- {
- motor[Intake1] = -110;
- motor[Intake2] = -110;
- }
- else
- {
- motor[Intake1] = 0;
- motor[Intake2] = 0;
- }
- //Pneumatics
- if( vexRT[ Btn8R ] == 1 )
- {
- if( ! buttonPressed1 )
- {
- buttonToggleState1 = 1 - buttonToggleState1;
- buttonPressed1 = 1;
- }
- }
- else
- {
- buttonPressed1 = 0;
- }
- if(buttonToggleState1)SensorValue[claw] = 1;
- else SensorValue[claw] = 0;
- if( vexRT[ Btn8U ] == 1 )
- {
- if( ! buttonPressed2 )
- {
- buttonToggleState2 = 1 - buttonToggleState2;
- buttonPressed2 = 1;
- }
- }
- else
- {
- buttonPressed2 = 0;
- }
- if(buttonToggleState2)SensorValue[platform] = 1;
- else SensorValue[platform] = 0;
- //Skyrise
- // UserControlCodePlaceholderForTesting();
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement