//---------------------------------------------------------
task control()
{
while(true)
{
motor[leftFrontMotor]=LeftFrontDrive;
motor[rightFrontMotor]=RightFrontDrive;
motor[rightBackMotor]=RightBackDrive;
motor[leftBackMotor]=LeftBackDrive;
int DriveFrontLeft=-1*(C1LY - C1LX - C1RX);
int DriveFrontRight=-1*(-C1LY - C1LX - C1RX);
int DriveBackRight=(-C1LY + C1LX - C1RX);
int DriveBackLeft=(C1LY + C1LX - C1RX);
float cubicPowerFrontLeft=cubicMap(DriveFrontLeft);
float cubicPowerFrontRight=cubicMap(DriveFrontRight);
float cubicPowerBackRight=cubicMap(DriveBackRight);
float cubicPowerBackLeft=cubicMap(DriveBackLeft);
RightFrontDrive = cubicPowerFrontRight;
RightBackDrive = cubicPowerBackRight;
LeftFrontDrive = cubicPowerFrontLeft;
LeftBackDrive = cubicPowerBackLeft;
abortTimeslice();
}
}
// controls movement of the robot
void UC_drive() {
if(vexRT[Btn8L]) {
setMorpheus();
} else if (vexRT[Btn8R]) {
setTank();
}
oldY = channel3;
channel1 = cubicMap(vexRT[Ch1]);
channel3 = cubicMap(vexRT[Ch3]);
channel4 = cubicMap(vexRT[Ch4]);
move(channel1, channel3, channel4);
}
void UC_drive() {
if (joystickGetDigital(1, 8, JOY_LEFT)) {
morpheus = 1;
} else if (joystickGetDigital(1, 8, JOY_LEFT)) {
morpheus = 0;
}
// Pneumatics
digitalWrite(RightMorpheus, morpheus);
digitalWrite(LeftMorpheus, morpheus);
// Basic movement
int channel1 = cubicMap(joystickGetAnalog(1, 1));
int channel3 = cubicMap(joystickGetAnalog(1, 3));
int channel4 = cubicMap(joystickGetAnalog(1, 4));
if (morpheus == 0) {
//motor(LeftFrontMotor1) = motor(LeftBackMotor1) = motor(LeftBackMotor2) = channel3 + TURN_SPEED * channel1;
//motor(RightFrontMotor1) = motor(RightBackMotor1) = motor(RightBackMotor2) = channel3 - TURN_SPEED * channel1;
// tank drive
float morpheus1 = channel3 + TURN_SPEED * channel1;
motorSet(LeftFrontMotor1, morpheus1);
motorSet(LeftBackMotor1, morpheus1);
motorSet(LeftBackMotor2, morpheus1);
float morpheus2 = channel3 - TURN_SPEED * channel1;
motorSet(RightFrontMotor1, morpheus2);
motorSet(RightBackMotor1, morpheus2);
motorSet(RightBackMotor2, morpheus2);
} else {
// x-drive
motorSet(LeftFrontMotor1, channel3 + channel4 + channel1);
motorSet(RightFrontMotor1, channel3 - channel4 - channel1);
motorSet(LeftBackMotor1, channel3 - channel4 + channel1);
motorSet(LeftBackMotor2, channel3 - channel4 + channel1);
motorSet(RightBackMotor1, channel3 + channel4 - channel1);
motorSet(RightBackMotor2, channel3 + channel4 - channel1);
}
}
task main()
{
while (true)
{
// Setting controller override
// Default - two controller
/* bool override = false;
if (vexRT[Btn7U] == 1 && vexRT[Btn8U] == 1)
{
override = !override;
}
*/
// Two controllers
// if (override == false) // Button to override
if (nVexRCReceiveState & vrXmit2)
{
{
/* Drive */
motor[leftBack] = cubicMap(vexRT[Ch2]+ vexRT[Ch4]);
motor[leftMiddle] = cubicMap(vexRT[Ch2]+ vexRT[Ch4]);
motor[leftFront] = cubicMap(vexRT[Ch2]+ vexRT[Ch4]);
motor[rightBack] = cubicMap(vexRT[Ch2]- vexRT[Ch4]);
motor[rightMiddle] = cubicMap(vexRT[Ch2]- vexRT[Ch4]);
motor[rightFront] = cubicMap(vexRT[Ch2]- vexRT[Ch4]);
/* Intake and Lift [Controller 2] */
// First Tier
motor[firstTierLeft] = vexRT[Ch2Xmtr2];
motor[firstTierRight] = vexRT[Ch2Xmtr2];
// Second Tier
motor[secondTier] = vexRT[Ch3Xmtr2];
/* Intake Rollers [Controller 2] */
// Intake push out
if (vexRT[Btn6UXmtr2] == 1)
{
motor[intakeRollers] = -127;
}
// Intake suck in
else if (vexRT[Btn5UXmtr2] == 1)
{
motor[intakeRollers] = 127;
}
// Unpressed
else
{
motor[intakeRollers] = 0;
}
}
}
// One controller
else
{
/* Drive */
motor[leftBack] = cubicMap(vexRT[Ch2]+ vexRT[Ch4]);
motor[leftMiddle] = cubicMap(vexRT[Ch2]+ vexRT[Ch4]);
motor[leftFront] = cubicMap(vexRT[Ch2]+ vexRT[Ch4]);
motor[rightBack] = cubicMap(vexRT[Ch2]- vexRT[Ch4]);
motor[rightMiddle] = cubicMap(vexRT[Ch2]- vexRT[Ch4]);
motor[rightFront] = cubicMap(vexRT[Ch2]- vexRT[Ch4]);
/* First Tier */
if (vexRT[Btn6U] == 1)
{
motor[firstTierLeft] = 127;
motor[firstTierRight] = 127;
}
else if (vexRT[Btn6D] == 1)
{
motor[firstTierLeft] = -127;
motor[firstTierRight] = -127;
}
else
{
motor[firstTierLeft] = 0;
motor[firstTierRight] = 0;
}
/* Second Tier */
if (vexRT[Btn5U] == 1)
{
motor[secondTier] = 127;
}
else if (vexRT[Btn5D] == 1)
{
motor[secondTier] = -127;
}
else
{
motor[secondTier] = 0;
}
/* Intake */
if (vexRT[Btn8R] == 1)
{
motor[intakeRollers] = 127;
}
else if (vexRT[Btn8D] == 1)
{
motor[intakeRollers] = -127;
}
else
{
motor[intakeRollers] = 0;
}
}
}
}
task usercontrol()
{
// User control code here, inside the loop
bool arcade = false;
while (true)
{
// This is the main execution loop for the user control program. Each time through the loop
// your program should update motor + servo values based on feedback from the joysticks.
// .....................................................................................
// Insert user code here. This is where you use the joystick values to update your motors, etc.
// .....................................................................................
if (vexRT[Btn7U] == 1)
{
progSkills();
}
if (vexRT[Btn8U] == 1)
{
arcade = true;
}
else if (vexRT[Btn8D] == 1)
{
arcade = false;
}
if (arcade == true)
{
motor[leftBack] = cubicMap(vexRT[Ch3] + vexRT[Ch1]);
motor[leftFront] = cubicMap(vexRT[Ch3] + vexRT[Ch1]);
motor[rightBack] = cubicMap(vexRT[Ch3] - vexRT[Ch1]);
motor[rightFront] = cubicMap(vexRT[Ch3] - vexRT[Ch1]);
}
else
{
motor[leftBack] = vexRT[Ch3];
motor[leftFront] = vexRT[Ch3];
motor[rightBack] = vexRT[Ch2];
motor[rightFront] = vexRT[Ch2];
}
if (abs(vexRT[Ch2Xmtr2]) > 10)
{
motor[leftArmBottom] = cubicMap(vexRT[Ch2Xmtr2]);
motor[leftArmTop] = cubicMap(vexRT[Ch2Xmtr2]);
motor[rightArmBottom] = cubicMap(vexRT[Ch2Xmtr2]);
motor[rightArmTop] = cubicMap(vexRT[Ch2Xmtr2]);
}
else if (vexRT[Btn6U] == 1)
{
motor[leftArmBottom] = 127;
motor[leftArmTop] = 127;
motor[rightArmBottom] = 127;
motor[rightArmTop] = 127;
}
else if (vexRT[Btn6D] == 1)
{
motor[leftArmBottom] = -127;
motor[leftArmTop] = -127;
motor[rightArmBottom] = -127;
motor[rightArmTop] = -127;
}
else
{
motor[leftArmBottom] = 0;
motor[leftArmTop] = 0;
motor[rightArmBottom] = 0;
motor[rightArmTop] = 0;
}
if (abs(vexRT[Ch3Xmtr2]) > 10)
{
motor[intakeLeft] = vexRT[Ch3Xmtr2];
motor[intakeRight] = vexRT[Ch3Xmtr2];
}
else if (vexRT[Btn5U] == 1)
{
motor[intakeLeft] = 127;
motor[intakeRight] = 127;
}
else if (vexRT[Btn5D] == 1)
{
motor[intakeLeft] = -127;
motor[intakeRight] = -127;
}
}
}
task usercontrol()
{
// User control code here, inside the loop
while (true)
{
while (true)
{
// Setting controller override
// Default - two controller
/* bool override = false;
if (vexRT[Btn7U] == 1 && vexRT[Btn8U] == 1)
{
override = !override;
}
*/
// Two controllers
// if (override == false) // Button to override
if (nVexRCReceiveState & vrXmit2)
{
{
/* Left Drive */
motor[leftFront] = vexRT[Ch3];
motor[leftMiddle] = vexRT[Ch3];
motor[leftBack] = vexRT[Ch3];
/* Right Drive */
motor[rightFront] = vexRT[Ch2];
motor[rightMiddle] = vexRT[Ch2];
motor[rightBack] = vexRT[Ch2];
/* Intake and Lift [Controller 2] */
// First Tier
motor[firstTierLeft] = vexRT[Ch2Xmtr2];
motor[firstTierRight] = vexRT[Ch2Xmtr2];
// Second Tier
motor[secondTier] = vexRT[Ch3Xmtr2];
/* Intake Rollers [Controller 2] */
// Intake push out
if (vexRT[Btn6UXmtr2] == 1)
{
motor[intakeRollers] = -127;
}
// Intake suck in
else if (vexRT[Btn5UXmtr2] == 1)
{
motor[intakeRollers] = 127;
}
// Unpressed
else
{
motor[intakeRollers] = 0;
}
}
}
// One controller
else
{
/* Drive */
motor[leftBack] = cubicMap(vexRT[Ch2]+ vexRT[Ch4]);
motor[leftMiddle] = cubicMap(vexRT[Ch2]+ vexRT[Ch4]);
motor[leftFront] = cubicMap(vexRT[Ch2]+ vexRT[Ch4]);
motor[rightBack] = cubicMap(vexRT[Ch2]- vexRT[Ch4]);
motor[rightMiddle] = cubicMap(vexRT[Ch2]- vexRT[Ch4]);
motor[rightFront] = cubicMap(vexRT[Ch2]- vexRT[Ch4]);
/* First Tier */
if (vexRT[Btn6U] == 1)
{
motor[firstTierLeft] = 127;
motor[firstTierRight] = 127;
}
else if (vexRT[Btn6D] == 1)
{
motor[firstTierLeft] = -127;
motor[firstTierRight] = -127;
}
else
{
motor[firstTierLeft] = 0;
motor[firstTierRight] = 0;
}
/* Second Tier */
if (vexRT[Btn5U] == 1)
{
motor[secondTier] = 127;
}
else if (vexRT[Btn5D] == 1)
{
motor[secondTier] = -127;
}
else
{
motor[secondTier] = 0;
}
/* Intake */
if (vexRT[Btn8R] == 1)
{
motor[intakeRollers] = 127;
}
else if (vexRT[Btn8D] == 1)
{
motor[intakeRollers] = -127;
}
else
{
motor[intakeRollers] = 0;
}
}
}
// This is the main execution loop for the user control program. Each time through the loop
// your program should update motor + servo values based on feedback from the joysticks.
// .....................................................................................
// Insert user code here. This is where you use the joystick values to update your motors, etc.
// .....................................................................................
}
}
