void Break(int direction){
switch (direction) {
case forward:
setDriveMotors(-127,-127,-127,-127);
wait10Msec(7);
break;
case turnLeft:
setDriveMotors(127, -127, -127, 127);
wait10Msec(7);
break;
case turnRight:
setDriveMotors(-127, 127, 127, -127);
wait10Msec(7);
break;
case backwards:
setDriveMotors(127,127,127,127);
wait10Msec(7);
break;
default:
setDriveMotors(0,0,0,0);
}
}
void turnDegrees(int degrees) // uses a PID loop to turn a given angle (clockwise) - used in autonomous
{
float goal = degrees * TICKS_PER_DEGREE; // conversion factor from degrees to encoder ticks
if (bBlueAlliance == true) // this reflects the turns if we're on the blue alliance
{
goal = goal * -1;
}
int output; // speed to send to the drive motots, set in the loop
initDriveEncoders(); // reset drive encoder counts to zero
InitPidGoal(turnPid, goal); // initialize the drive PID with the goal
ClearTimer(T2); // clear the timer
while( (abs(turnPid.error) > turnErrorThreshold)
|| (abs(turnPid.derivative) > turnDerivativeThreshold) ) // until both the error and speed drop below acceptable thresholds...
{
if (time1(T2) > PID_UPDATE_TIME) // if enough time has passed since the last PID update...
{
float currentPosition = (nMotorEncoder[leftBackDrive]-nMotorEncoder[rightBackDrive]) / 2.0; // (the current position is the (+) average of the drive encoders)
output = UpdatePid(turnPid, currentPosition); // ...update the motor speed with the drive PID...
setDriveMotors(-output, output); // ...and send that speed to the drive motors (left and right)
ClearTimer(T2); // clear the timer
}
}
setDriveMotors(0, 0); // remember to stop the motors!
wait1Msec(TIME_BETWEEN_AUTONOMOUS_ACTIONS); // lets things settle before moving to next action
}
void deployIntake() // jerks the robot back and forth to deploy the intake arms
{
setDriveMotors(127,127);
wait1Msec(100);
setDriveMotors(-127,-127);
wait1Msec(100);
setDriveMotors(0,0);
wait1Msec(TIME_BETWEEN_AUTONOMOUS_ACTIONS); // lets things settle before moving to next action
}
void move(int direction, int speed, int distance, int FBbreakspd){
revolutions = 360*(distance/wheelCircumference);
switch (direction) {
case forward:
zeroEncoders();
while(getDTEncoderAverage() < revolutions){
setDriveMotors(127,127,127, 127);
setFBMotors(FBbreakspd);
}
zeroEncoders();
Break(forward);
break;
case turnLeft: //int FLSpeed, int FRspeed, int BRspeed, int BLspeed
zeroEncoders();
while(getDTEncoderAverage() < revolutions){
setDriveMotors(-127, 127, 127, -127);
setFBMotors(FBbreakspd);
}
Break(turnLeft);
break;
case turnRight: //int FLSpeed, int FRspeed, int BRspeed, int BLspeed
zeroEncoders();
while(getDTEncoderAverage() < revolutions){
setDriveMotors(127, -127, -127, 127);
setFBMotors(FBbreakspd);
}
Break(turnRight);
break;
case backwards:
zeroEncoders();
while(getDTEncoderAverage() < revolutions){
setDriveMotors(-127, -127, -127, -127);
setFBMotors(FBbreakspd);
}
Break(backwards);
break;
default:
setDriveMotors(0,0,0,0);
zeroEncoders();
}
zeroEncoders();
}
void update()
{
processHandlerState();
setDriveMotors();
setFlipMotor();
updateDashboard(handlerState, armState);
}
void Drive::arcade(float move_, float rotate_)
{
float move = signSquare(limit(move_));
float rotate = signSquare(limit(rotate_));
float left, right;
if (move < 0)
{
if (rotate > 0)
{
left = (-move - rotate);
right = max(-move, rotate);
}
else
{
left = max(-move, -rotate);
right = (-move + rotate);
}
}
else
{
if (rotate > 0)
{
left = -max(move, rotate);
right = (-move + rotate);
}
else
{
left = (-move - rotate);
right = -max(move, -rotate);
}
}
setDriveMotors(left, right);
}
void rightStar(){
//Left Square Star Shooting
move(backwards, 50, 5, 0); //move back to allow room to release platform
dotheShimmy(); //release the platform
move(forward, 75, 45, 0); //move to pick up stars
//Secure the stars by lifting up the platform
setDriveMotors(0,0,0,0);
moveFB(up, 400, 120);
move(backwards, 127, 25, fbbs);
//Get in position to launch the stars
move(turnLeft, 40, 11.5, fbbs); //Rotate 90 degrees clockwise
move(backwards, 10, 45, fbbs); //Drive backwards toward the fence
setDriveMotors(0,0,0,0);
wait10Msec(2);
//Launch the stars
moveFB(up, 600, 127);
}
void setDrive() // reads Joystick, calculates drive motor values and sends values to motors - used in driver control
{
int driveJoystickY = deadband(vexRT[Ch3], DRIVE_DEADBAND_THRESHOLD); // read from the left josytick Y-axis (channel 3) and deadband value
int driveJoystickX = deadband(vexRT[Ch4], DRIVE_DEADBAND_THRESHOLD); // read from the left josytick X-axis (channel 4) and deadband value
int rightMotorValue = driveJoystickY - driveJoystickX; // arcade drive left side value
int leftMotorValue = driveJoystickY + driveJoystickX; // arcade drive right side value
setDriveMotors(rightMotorValue, leftMotorValue); // send the calculated motor values to the drive sides
}
void driveTank(int left, int right, bool square, bool ramp){
if(square)
{
if(left!=0)
{
leftDrivePower = ((left*left)/127)*(left/abs(left));
}
else
{
leftDrivePower = 0;
}
if(right!=0)
{
rightDrivePower = ((right*right)/127)*(right/abs(right));
}
else
{
rightDrivePower = 0;
}
}
else
{
leftDrivePower = left;
rightDrivePower = right;
}
if(ramp == true)
{
if(abs(rightDrivePower) > abs(rightDrivePrevious) + rampLimit)
{
if(rightDrivePower > (rightDrivePower + rampLimit))
{
rightDrivePower = rightDrivePrevious + rampLimit;
}
else
{
rightDrivePower = rightDrivePrevious - rampLimit;
}
}
if(abs(leftDrivePower) > abs(leftDrivePrevious) + rampLimit){
if(leftDrivePower > (leftDrivePower + rampLimit)){
leftDrivePower = leftDrivePrevious + rampLimit;
}
else{
leftDrivePower = leftDrivePrevious - rampLimit;
}
}
}
setDriveMotors();
}
void driveInches(int inches) // uses a PID loop to drive (straight) a given distance - used in autonomous
{
float goal = inches * TICKS_PER_INCH; // conversion factor from inches to encoder ticks
int output; // speed to send to the drive motots, set in the loop
initDriveEncoders(); // reset drive encoder counts to zero
InitPidGoal(drivePid, goal); // initialize the drive PID with the goal
ClearTimer(T1); // clear the timer
while( (abs(drivePid.error) > driveErrorThreshold)
|| (abs(drivePid.derivative) > driveDerivativeThreshold) ) // until both the error and speed drop below acceptable thresholds...
{
if (time1(T1) > PID_UPDATE_TIME) // if enough time has passed since the last PID update...
{
float currentPosition = (-nMotorEncoder[leftBackDrive]-nMotorEncoder[rightBackDrive]) / 2.0; // (the current position is the average of the drive encoders)
output = UpdatePid(drivePid, currentPosition); // ...update the motor speed with the drive PID...
float offset = DRIVE_STRAIGHT_FACTOR * ( (-nMotorEncoder[leftBackDrive]) - (-nMotorEncoder[rightBackDrive]) ) ; // (proportional correction to straighten out)
setDriveMotors(output - offset, output + offset); // ...and send that speed to the drive motors (left and right)
ClearTimer(T1); // reset the timer so it starts counting again
}
}
setDriveMotors(0, 0); // remember to stop the motors!
wait1Msec(TIME_BETWEEN_AUTONOMOUS_ACTIONS); // lets things settle before moving to next action
}
task autonomous()
{
/*------------------------------------------------------------------------
Autonomous Play #1 - Fence
Play-by-Play
Pre-Match
1. Place robot FORWARD FACING
2. Set dial to LFen or RFen to run playImmediateTone
(A) Left Field
1. Rotate four-bar 80 degrees up (positive speed value)
2. Move forward 40 inches (give or take) which knocks off two stars (+2 points)
3. Move backwards 3.25 inches (1 wheel rotation)
4. Rotate 90 degrees clockwise
5. Move forward 15 inches and knock off 1-2 more stars
6. Stop
(B) Right Field
1. Rotate four-bar 80 degrees up (positive speed value)
2. Move forward 40 inches (give or take) which knocks off two stars (+2 points)
3. Move backwards 3.25 inches (1 wheel rotation)
4. Rotate 90 degrees counterclockwise
5. Move forward 15 inches and knock off 1-2 more stars
6. Stop
Total Points: 2
------------------------------------------------------------------------*/
/*------------------------------------------------------------------------
Autonomous Play #2 - Scoring Stars in the Far Zone
Pre-Match
1. Place robot facing right (left field) or left (right field)
2. Set dial to LStar or RStar to run playImmediateTone
(A) Left Field
1. Move backwards 12 inches
2. Do the shimmy to lower platform
3. Move forward 36 inches
4. Lift platform 30 degree or until stars clear the fence (+6 pts)
5. Move backward 36" (to clear cube)
6. Rotate 90 degrees clockwise
7. Move backwards roughly 38 inches
9. Lower platform 30 degrees then rotate 90 deg up to score in far zone (+6 pts)
(B) Right Field
1. Move backwards 12 inches
2. Do the shimmy to lower platform
3. Move forward 36 inches
4. Lift platform 30 degree or until stars clear the fence (+6 pts)
5. Move backward 36" (to clear cube)
6. Rotate 90 degrees counterclockwise
7. Move backwards roughly 38 inches
9. Lower platform 30 degrees then rotate 90 deg up to score in far zone (+6 pts)
Total Points: 12
------------------------------------------------------------------------*/
//Auton Switching Wizard
switch (getPTVal()){
case 0:
leftFence();
break;
case 1:
rightFence();
break;
case 2:
leftStar();
break;
case 3:
rightStar();
break;
case 4:
autonSkills();
break;
default:
setDriveMotors(0,0,0,0);
}
}
void encoderTest(int ticks){
zeroEncoders();
while(getDTEncoderAverage() < ticks){
setDriveMotors(127,127,127, 127);
}
}
void Drive::CheesyDrive(double throttle, double wheel, bool highGear, bool quickTurn) {
bool isQuickTurn = quickTurn;
float turnNonlinHigh = 0.2; // smaller is more responsive bigger is less responsive
float turnNonlinLow = 0.8;
float negInertiaHigh = 1.2; // bigger is more responsive
float senseHigh = 1.2;
float senseLow = 0.6;
float senseCutoff = 0.1;
float negInertiaLowMore = 2.5;
float negInertiaLowLessExt = 5.0;
float negInertiaLowLess = 3.0;
float quickStopTimeConstant = 0.1;
float quickStopLinearPowerThreshold = 0.2;
float quickStopStickScalar = 0.8; //Bigger for faster stopping
double wheelNonLinearity;
double negInertia = wheel - oldWheel;
oldWheel = wheel;
if (highGear) {
wheelNonLinearity = turnNonlinHigh;
// Apply a sin function that's scaled to make it feel better.
wheel = sin(M_PI / 2.0 * wheelNonLinearity * wheel) / sin(M_PI / 2.0 * wheelNonLinearity);
wheel = sin(M_PI / 2.0 * wheelNonLinearity * wheel) / sin(M_PI / 2.0 * wheelNonLinearity);
} else {
wheelNonLinearity = turnNonlinLow;
// Apply a sin function that's scaled to make it feel better.
wheel = sin(M_PI / 2.0 * wheelNonLinearity * wheel) / sin(M_PI / 2.0 * wheelNonLinearity);
wheel = sin(M_PI / 2.0 * wheelNonLinearity * wheel) / sin(M_PI / 2.0 * wheelNonLinearity);
wheel = sin(M_PI / 2.0 * wheelNonLinearity * wheel) / sin(M_PI / 2.0 * wheelNonLinearity);
}
double leftPwm, rightPwm, overPower;
float sensitivity = 1.7;
float angularPower;
float linearPower;
// Negative inertia!
double negInertiaScalar;
if (highGear) {
negInertiaScalar = negInertiaHigh;
sensitivity = senseHigh;
} else {
if (wheel * negInertia > 0) {
negInertiaScalar = negInertiaLowMore;
} else {
if (fabs(wheel) > 0.65) {
negInertiaScalar = negInertiaLowLessExt;
} else {
negInertiaScalar = negInertiaLowLess;
}
}
sensitivity = senseLow;
if (fabs(throttle) > senseCutoff) {
sensitivity = 1 - (1 - sensitivity) / fabs(throttle);
}
}
double negInertiaPower = negInertia * negInertiaScalar;
negInertiaAccumulator += negInertiaPower;
wheel = wheel + negInertiaAccumulator;
if(negInertiaAccumulator > 1)
negInertiaAccumulator -= 1;
else if (negInertiaAccumulator < -1)
negInertiaAccumulator += 1;
else
negInertiaAccumulator = 0;
linearPower = throttle;
// Quickturn!
if (isQuickTurn) {
if (fabs(linearPower) < quickStopLinearPowerThreshold) {
double alpha = quickStopTimeConstant;
quickStopAccumulator = (1 - alpha) * quickStopAccumulator + alpha * limit(wheel) * quickStopStickScalar;
}
overPower = 1.0;
if (highGear) {
sensitivity = 1.0;
} else {
sensitivity = 1.0;
}
angularPower = wheel;
} else {
overPower = 0.0;
angularPower = fabs(throttle) * wheel * sensitivity - quickStopAccumulator;
if (quickStopAccumulator > 1) {
quickStopAccumulator -= 1;
} else if (quickStopAccumulator < -1) {
quickStopAccumulator += 1;
} else {
quickStopAccumulator = 0.0;
}
}
rightPwm = leftPwm = linearPower;
leftPwm += angularPower;
rightPwm -= angularPower;
if (leftPwm > 1.0) {
rightPwm -= overPower * (leftPwm - 1.0);
leftPwm = 1.0;
} else if (rightPwm > 1.0) {
leftPwm -= overPower * (rightPwm - 1.0);
rightPwm = 1.0;
} else if (leftPwm < -1.0) {
rightPwm += overPower * (-1.0 - leftPwm);
leftPwm = -1.0;
} else if (rightPwm < -1.0) {
leftPwm += overPower * (-1.0 - rightPwm);
rightPwm = -1.0;
}
setDriveMotors(leftPwm, rightPwm);
setLowGear(highGear);
}
//--------------------Manual Control Loop--------------------//
task usercontrol() {
targetSpeed = optimalSpeed;
SensorValue[shootSolenoid] = 0; //Set the shooter to open
manualSetSpeed = defaultManualSpeed;
setShooterMotors(0);
clearTimer(T3);
while (true) {
int x = vexRT[joyDriveA];
int y = vexRT[joyDriveB];
int z = (vexRT[joyTurnRight] - vexRT[joyTurnLeft]) * 127 + (vexRT[joyTurnRightSlow] - vexRT[joyTurnLeftSlow]) * 50;
calculateShooter();
if (time1[T3] < 300) {shooterMotorRaw = 767;} //Set the power briefly to max after shooting a ball
//The global shooterMotorRaw holds the power globally. it is passed to setShooterMotors. setShooterMotors should
//accept the power and set the motors accordingly
setShooterMotors(shooterMotorRaw); //set the shooter motor's power
solenoidsManual(); //Get button innputs for solenoid control
//Basic configuration for 4 meccanum wheel drive
setDriveMotors(x + z + y,
x - z - y,
x + z - y,
x - z + y);
//bring the shooter to a full stop permanently
if (vexRT[joyShooterZero] == 1) {
shooterMotorRaw = 0;
setShooterMotors(0);
manualSetSpeed = 0;
targetSpeed = 0;
//Increment the target
}
else if (vexRT[joyShooterIncU] == 1) {
manualSetSpeed+= 1;
//Decrement the target
}
else if (vexRT[joyShooterIncD] == 1) {
manualSetSpeed-=1;
//Set the shooter speed to the optimal speed
}
else if (vexRT[joyShooterFull] == 1) {
manualSetSpeed = defaultManualSpeed;
//targetSpeed = optimalSpeed;
} //shooter button if statements
//MANAL SPEED INCREMENT
if (vexRT[joyShooterSpIU] == 1) {
targetSpeed+= 0.2;
//Decrement the target
}
else if (vexRT[joyShooterSpID] == 1) {
targetSpeed-=0.2;
//Set the shooter speed to the optimal speed
}
//Record the time since the last ball was shot
if (SensorValue[ballDetect] <= ballDetectThreshold && time1[T3] > 800) {
lastShootTime = time1[T3];
writeDebugStreamLine("%i",lastShootTime);
clearTimer(T3);
}
}
}
void driveArcade(int power, int turn, bool square, bool ramp){
if(square)
{
if(power != 0)
{
y = ((power*power)/127)*(power/abs(power));
}
else
{
y = 0;
}
if(turn!=0)
{
x = ((turn*turn)/127)*(turn/abs(turn));
}
else
{
x = 0;
}
}
else
{
y = power;
x = turn;
}
leftDrivePower = y+x;
rightDrivePower = y-x;
if(ramp == true)
{
if(abs(rightDrivePower) > abs(rightDrivePrevious) + rampLimit)
{
if(rightDrivePower > (rightDrivePower + rampLimit))
{
rightDrivePower = rightDrivePrevious + rampLimit;
}
else
{
rightDrivePower = rightDrivePrevious - rampLimit;
}
}
if(abs(leftDrivePower) > abs(leftDrivePrevious) + rampLimit)
{
if(leftDrivePower > (leftDrivePower + rampLimit))
{
leftDrivePower = leftDrivePrevious + rampLimit;
}
else
{
leftDrivePower = leftDrivePrevious - rampLimit;
}
}
}
setDriveMotors();
leftDrivePrevious = leftDrivePower;
rightDrivePrevious = rightDrivePower;
}
void stopDrive(){
rightDrivePower = leftDrivePower = 0;
setDriveMotors();
}
