#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, in1, gyro, sensorGyro)
#pragma config(Sensor, dgtl3, LimitSwitch, sensorTouch)
#pragma config(Sensor, I2C_1, leftDriveCount, sensorQuadEncoderOnI2CPort, , AutoAssign )
#pragma config(Sensor, I2C_2, rightDriveCount, sensorQuadEncoderOnI2CPort, , AutoAssign )
#pragma config(Motor, port2, rightDrive, tmotorVex393HighSpeed_MC29, openLoop, reversed, driveRight, encoderPort, I2C_2)
#pragma config(Motor, port3, leftDrive, tmotorVex393HighSpeed_MC29, openLoop, reversed, driveLeft, encoderPort, I2C_1)
#pragma config(Motor, port4, rightTopLauncher, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port5, rightMiddleLauncher, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port6, bottomLauncher, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port7, leftMiddleLauncher, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port8, leftTopLauncher, tmotorVex393_MC29, openLoop, reversed)
int cubicMap(int x){
return x / 127 * x / 127 * x;
}
task usercontrol()
{
while(true){ // arcade drive user control
// left joystick Ch3 fw-bk
// right joystick Ch1 lt-rt
int forward = 0; int backward = 0;
if(vexRT[Ch3] > 0){
int value = cubicMap(vexRT[Ch3] + abs(vexRT[Ch4]));
if(value > 80){
int currentSpeed = 80;
while(currentSpeed < value){
forward = currentSpeed;
motor[leftDrive] = (forward + leftRight) / 2;
currentSpeed += (value - currentSpeed) / 4;
wait1Msec(150);
}
} else{
forward = cubicMap(vexRT[Ch3] + abs(vexRT[Ch4]));
}
} else{
int value = cubicMap(vexRT[Ch3] - abs(vexRT[Ch4]));
if(value > 80){
int currentSpeed = 80;
while(currentSpeed < value){
backward = currentSpeed;
motor[leftDrive] = (backward + leftRight) / 2;
currentSpeed += (value - currentSpeed) / 4;
wait1Msec(150);
}
} else{
backward = cubicMap(vexRT[Ch3] - abs(vexRT[Ch4]));
}
}
int leftRight = 0;
if(vexRT[Ch1] > 0){
leftRight = cubicMap(vexRT[Ch1] + abs(vexRT[Ch2]));
} else{
leftRight = cubicMap(vexRT[Ch1] - abs(vexRT[Ch2]));
}
motor[leftDrive] = (forward + backward + leftRight) / 2;
motor[rightDrive] = (forward + backward - leftRight) / 2;
if(vexRT[Btn6U] == 1){ // raise arm
motor[rightTopLauncher] = motor[rightMiddleLauncher] = motor[bottomLauncher] = motor[leftMiddleLauncher] = motor[leftTopLauncher] = 127;
} else if(vexRT[Btn6D] == 1){ // pull down arm
motor[rightTopLauncher] = motor[rightMiddleLauncher] = motor[bottomLauncher] = motor[leftMiddleLauncher] = motor[leftTopLauncher] = -127;
} else if(SensorValue[LimitSwitch] == 1 && vexRT[Btn6U] == 0) { // hold down arm
motor[rightTopLauncher] = motor[rightMiddleLauncher] = motor[bottomLauncher] = motor[leftMiddleLauncher] = motor[leftTopLauncher] = -10;
} else{ // stop moving arm
motor[rightTopLauncher] = motor[rightMiddleLauncher] = motor[bottomLauncher] = motor[leftMiddleLauncher] = motor[leftTopLauncher] = 0;
}
}
}
Thanks!