void positionArm(int position)
{// Positions arm to a specified place
if (getArmPotentiometer() < position )
{
while(getArmPotentiometer() < position && vexRT[Btn8R] == 0)
{
raiseArm();
driveWithJoysticks();
intakeControl();
}
stopArm();
}
else if (getArmPotentiometer() > position)
{
while (getArmPotentiometer() > position && vexRT[Btn8R] == 0)
{
lowerArm();
driveWithJoysticks();
intakeControl();
}
stopArm();
}
}
void armHelixUp (double spikeNumber)
{
const Encoder TOWER_ENCODER = encoderInit (2,3,false);
armUpDead(); // power all lift motors to max
motorSet (SWING_MOTOR, SWING_MOTOR_SPEED);
bool done =false, swingDone =false, heightDone =false;
int towerCount=0, swingCount=0;
while(!done)
{
swingCount = analogRead (SWING_POT_PIN); //4095 is 5 volts
towerCount = encoderGet(TOWER_ENCODER);//documentation claims 360 ticks in 1 revolution
if (towerCount > SPIKE_OFFSET * spikeNumber)
{
stopArm();
printf("Tower raised, might fall, motor stopped\n");
heightDone = true;
}
if (swingCount > SWING_ANGLE)
{
motorSet (SWING_MOTOR, 0);
printf("Arm swung, may be wrong angle\n");
swingDone=true;
}
done = swingDone && heightDone;
}
}
///Move to loading height
void armHelixDown()
{
motorSet (SWING_MOTOR, -SWING_MOTOR_SPEED);
armDownDead(); // power all lift motors to max
bool done =false, swingDone =false, heightDone =false;
int towerCount=0, swingCount=0;
const Encoder TOWER_ENCODER = encoderInit (TOWER_ENCODER_TOP_CHANNEL,TOWER_ENCODER_BOTTOM_CHANNEL,false);
while(!done)
{
swingCount = analogRead (SWING_POT_PIN); //4095 is 5 volts
towerCount = encoderGet(TOWER_ENCODER);//documentation claims 360 ticks in 1 revolution
if (towerCount < SPIKE_OFFSET*1.25) //1.25 is an estimate for the load height
{
stopArm();
printf("Tower lowered to load position\n");
heightDone = true;
}
if (swingCount < 100) //TODo no measurements for pot yet
{
motorSet (SWING_POT_PIN, 0);
printf("Arm swung, may be wrong angle\n");
swingDone=true;
}
done = swingDone && heightDone;
}
}
// Controls the algorithm in determining arm speed
void moveArm(int target) {
if(target > 0) {
int tempSpeed = armSpeedPrevious + ((127-armSpeedPrevious)/5);
powerArm(tempSpeed);
armSpeedPrevious = tempSpeed;
} else if (target < 0) {
int tempSpeed = armSpeedPrevious - ((127+armSpeedPrevious)/5);
powerArm(tempSpeed);
armSpeedPrevious = tempSpeed;
} else {
stopArm();
}
}
// controls the algorithm in determining arm speed
void moveArm(int target) {
if(target > 0 /*&& armRotation < 1250*/) {
int tempSpeed = armSpeedPrevious + ((127-armSpeedPrevious)/5);
powerArm(tempSpeed);
armSpeedPrevious = tempSpeed;
} else if (target < 0 && armRotation > 0) {
int tempSpeed = armSpeedPrevious - ((127+armSpeedPrevious)/5);
powerArm(tempSpeed);
armSpeedPrevious = tempSpeed;
} else {
stopArm();
}
}
void armDown(int x)
{
int pot = analogRead (8);
while (pot > x)
{
motorSet (motor3, 127) ; // arm down
motorSet (motor4, -127) ;
motorSet (motor7, 127) ;
motorSet (motor8, -127) ;
pot = analogRead (8);
}
stopArm();
}
/*void driveForward(int x) {
int imeAccumulator = 0;
imeReset(0); // rest rightLine I.M.E
//Read decoder into counts
imeGet(0, &imeAccumulator);
//Move forward at max speed until desired x
while (abs(imeAccumulator) < x) {
motorSet(MOTOR_DRIVE_RIGHT_BACK, FORWARD_VELOCITY);
motorSet(MOTOR_DRIVE_RIGHT_FRONT, FORWARD_VELOCITY);
motorSet(MOTOR_ARM_LEFT_BACK, FORWARD_VELOCITY);
motorSet(MOTOR_ARM_LEFT_FRONT, FORWARD_VELOCITY);
imeGet(0, &imeAccumulator); // keep getting the value
}
//Cancel forward inertia
motorSet(MOTOR_DRIVE_RIGHT_BACK, -INERTIA_CANCELLATION_FACTOR); // no inertia
motorSet(MOTOR_DRIVE_RIGHT_FRONT, -INERTIA_CANCELLATION_FACTOR);
motorSet(MOTOR_ARM_LEFT_BACK, -INERTIA_CANCELLATION_FACTOR);
motorSet(MOTOR_ARM_LEFT_FRONT, -INERTIA_CANCELLATION_FACTOR);
delay(65);
motorSet(MOTOR_DRIVE_RIGHT_BACK, 0);
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 0);
motorSet(MOTOR_ARM_LEFT_BACK, 0);
motorSet(MOTOR_ARM_LEFT_FRONT, 0);
delay(200);
}
void driveBack(int x) {
int counts = 0;
imeReset(0); // rest rightLine I.M.E
imeGet(0, &counts);
while (abs(counts) < x) {
motorSet(MOTOR_DRIVE_RIGHT_BACK, REVERSE_VELOCITY);
motorSet(MOTOR_DRIVE_RIGHT_FRONT, REVERSE_VELOCITY);
motorSet(MOTOR_ARM_LEFT_BACK, REVERSE_VELOCITY);
motorSet(MOTOR_ARM_LEFT_FRONT, REVERSE_VELOCITY);
imeGet(0, &counts); // keep getting the value
}
motorSet(MOTOR_DRIVE_RIGHT_BACK, INERTIA_CANCELLATION_FACTOR); // no inertia
motorSet(MOTOR_DRIVE_RIGHT_FRONT, INERTIA_CANCELLATION_FACTOR);
motorSet(MOTOR_ARM_LEFT_BACK, INERTIA_CANCELLATION_FACTOR);
motorSet(MOTOR_ARM_LEFT_FRONT, INERTIA_CANCELLATION_FACTOR);
delay(65);
motorStop(MOTOR_DRIVE_RIGHT_BACK);
motorStop(MOTOR_DRIVE_RIGHT_FRONT);
motorStop(MOTOR_ARM_LEFT_BACK);
motorStop(MOTOR_ARM_LEFT_FRONT);
delay(200);
}
void turnRight(int x) {
int counts = 0;
imeReset(0); // rest rightLine I.M.E
imeGet(0, &counts);
while (abs(counts) < x) {
motorSet(MOTOR_DRIVE_RIGHT_BACK, -64);
motorSet(MOTOR_DRIVE_RIGHT_FRONT, -64);
motorSet(MOTOR_ARM_LEFT_BACK, 64);
motorSet(MOTOR_ARM_LEFT_FRONT, 64);
imeGet(0, &counts); // keep getting the value
}
motorSet(MOTOR_DRIVE_RIGHT_BACK, 10); // no inertia
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 10);
motorSet(MOTOR_ARM_LEFT_BACK, -10);
motorSet(MOTOR_ARM_LEFT_FRONT, -10);
delay(45);
motorStop(MOTOR_DRIVE_RIGHT_BACK);
motorStop(MOTOR_DRIVE_RIGHT_FRONT);
motorStop(MOTOR_ARM_LEFT_BACK);
motorStop(MOTOR_ARM_LEFT_FRONT);
delay(200);
}
void turnLeft(int x) {
int counts = 0;
imeReset(0); // rest rightLine I.M.E
imeGet(0, &counts);
while (abs(counts) < x) {
motorSet(MOTOR_DRIVE_RIGHT_BACK, 64);
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 64);
motorSet(MOTOR_ARM_LEFT_BACK, -64);
motorSet(MOTOR_ARM_LEFT_FRONT, -64);
imeGet(0, &counts); // keep getting the value
}
motorSet(MOTOR_DRIVE_RIGHT_BACK, -10); // no inertia
motorSet(MOTOR_DRIVE_RIGHT_FRONT, -10);
motorSet(MOTOR_ARM_LEFT_BACK, 10);
motorSet(MOTOR_ARM_LEFT_FRONT, 10);
delay(45);
motorStop(MOTOR_DRIVE_RIGHT_BACK);
motorStop(MOTOR_DRIVE_RIGHT_FRONT);
motorStop(MOTOR_ARM_LEFT_BACK);
motorStop(MOTOR_ARM_LEFT_FRONT);
delay(200);
}
void turnRightSlowDead() {
motorSet(MOTOR_DRIVE_RIGHT_BACK, -50);
motorSet(MOTOR_DRIVE_RIGHT_FRONT, -50);
motorSet(MOTOR_ARM_LEFT_BACK, 50);
motorSet(MOTOR_ARM_LEFT_FRONT, 50);
}
void turnLeftSlowDead() {
motorSet(MOTOR_DRIVE_RIGHT_BACK, 40);
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 40);
motorSet(MOTOR_ARM_LEFT_BACK, -40);
motorSet(MOTOR_ARM_LEFT_FRONT, -40);
}
void turnLeftArc(int x) {
int counts = 0;
imeReset(0); // rest rightLine I.M.E
imeGet(0, &counts);
while (abs(counts) < x) {
motorSet(MOTOR_DRIVE_RIGHT_BACK, 127);
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 127);
motorSet(MOTOR_ARM_LEFT_BACK, 0);
motorSet(MOTOR_ARM_LEFT_FRONT, 0);
imeGet(0, &counts); // keep getting the value
}
motorStop(MOTOR_DRIVE_RIGHT_BACK);
motorStop(MOTOR_DRIVE_RIGHT_FRONT);
motorStop(MOTOR_ARM_LEFT_BACK);
motorStop(MOTOR_ARM_LEFT_FRONT);
delay(200);
}
void turnRightArc(int x) {
int counts = 1;
imeReset(1); // rest rightLine I.M.E
imeGet(1, &counts);
while (abs(counts) < x) {
motorSet(MOTOR_DRIVE_RIGHT_BACK, 0);
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 0);
motorSet(MOTOR_ARM_LEFT_BACK, 127);
motorSet(MOTOR_ARM_LEFT_FRONT, 127);
imeGet(0, &counts); // keep getting the value
}
motorStop(MOTOR_DRIVE_RIGHT_BACK);
motorStop(MOTOR_DRIVE_RIGHT_FRONT);
motorStop(MOTOR_ARM_LEFT_BACK);
motorStop(MOTOR_ARM_LEFT_FRONT);
}
void stopDrive() {
motorStop(MOTOR_DRIVE_RIGHT_BACK);
motorStop(MOTOR_DRIVE_RIGHT_FRONT);
motorStop(MOTOR_ARM_LEFT_BACK);
motorStop(MOTOR_ARM_LEFT_FRONT);
delay(200);
}
void stopAll() {
motorStop(MOTOR_DRIVE_RIGHT_BACK);
motorStop(MOTOR_DRIVE_RIGHT_FRONT);
motorStop(MOTOR_ARM_RIGHT_BOTTOM);
motorStop(MOTOR_ARM_RIGHT_TOP);
motorStop(MOTOR_ARM_RIGHT_MID);
motorStop(MOTOR_ARM_LEFT_MID);
motorStop(MOTOR_ARM_LEFT_TOP);
motorStop(MOTOR_ARM_LEFT_BOTTOM);
motorStop(MOTOR_ARM_LEFT_FRONT);
motorStop(MOTOR_ARM_LEFT_BACK);
}
void stopIntake() {
motorStop(MOTOR_ARM_RIGHT_MID);
motorStop(MOTOR_ARM_LEFT_MID);
}
void driveForwardDead() {
motorSet(MOTOR_DRIVE_RIGHT_BACK, 127);
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 127);
motorSet(MOTOR_ARM_LEFT_BACK, 127);
motorSet(MOTOR_ARM_LEFT_FRONT, 127);
}
void driveBackDead() {
motorSet(MOTOR_DRIVE_RIGHT_BACK, -127);
motorSet(MOTOR_DRIVE_RIGHT_FRONT, -127);
motorSet(MOTOR_ARM_LEFT_BACK, -127);
motorSet(MOTOR_ARM_LEFT_FRONT, -127);
}
void driveForwardSlow(int x) {
int counts = 0;
imeReset(0); // rest rightLine I.M.E
imeGet(0, &counts);
while (abs(counts) < x) {
motorSet(MOTOR_DRIVE_RIGHT_BACK, 40);
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 40);
motorSet(MOTOR_ARM_LEFT_BACK, 40);
motorSet(MOTOR_ARM_LEFT_FRONT, 40);
imeGet(0, &counts); // keep getting the value
}
motorSet(MOTOR_DRIVE_RIGHT_BACK, -10); // no inertia
motorSet(MOTOR_DRIVE_RIGHT_FRONT, -10);
motorSet(MOTOR_ARM_LEFT_BACK, -10);
motorSet(MOTOR_ARM_LEFT_FRONT, -10);
delay(45);
motorSet(MOTOR_DRIVE_RIGHT_BACK, 0);
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 0);
motorSet(MOTOR_ARM_LEFT_BACK, 0);
motorSet(MOTOR_ARM_LEFT_FRONT, 0);
delay(200);
}
void driveBackSlow(int x) {
int counts = 0;
imeReset(0); // rest rightLine I.M.E
imeGet(0, &counts);
while (abs(counts) < x) {
motorSet(MOTOR_DRIVE_RIGHT_BACK, -40);
motorSet(MOTOR_DRIVE_RIGHT_FRONT, -40);
motorSet(MOTOR_ARM_LEFT_BACK, -40);
motorSet(MOTOR_ARM_LEFT_FRONT, -40);
imeGet(0, &counts); // keep getting the value
}
motorSet(MOTOR_DRIVE_RIGHT_BACK, 10); // no inertia
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 10);
motorSet(MOTOR_ARM_LEFT_BACK, 10);
motorSet(MOTOR_ARM_LEFT_FRONT, 10);
delay(45);
motorSet(MOTOR_DRIVE_RIGHT_BACK, 0);
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 0);
motorSet(MOTOR_ARM_LEFT_BACK, 0);
motorSet(MOTOR_ARM_LEFT_FRONT, 0);
delay(200);
}
void driveForwardSlowDead() {
motorSet(MOTOR_DRIVE_RIGHT_BACK, 50);
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 50);
motorSet(MOTOR_ARM_LEFT_BACK, 50);
motorSet(MOTOR_ARM_LEFT_FRONT, 50);
}
void driveBackSlowDead() {
motorSet(MOTOR_DRIVE_RIGHT_BACK, -40);
motorSet(MOTOR_DRIVE_RIGHT_FRONT, -40);
motorSet(MOTOR_ARM_LEFT_BACK, -40);
motorSet(MOTOR_ARM_LEFT_FRONT, -40);
}
void scrapeRight(int x) {
int counts = 0;
imeReset(1); // rest left I.M.E
imeGet(1, &counts);
while (abs(counts) < x) {
motorSet(MOTOR_DRIVE_RIGHT_BACK, 0);
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 0);
motorSet(MOTOR_ARM_LEFT_BACK, 90);
motorSet(MOTOR_ARM_LEFT_FRONT, 90);
imeGet(1, &counts); // keep getting the value
}
motorSet(MOTOR_DRIVE_RIGHT_BACK, 0);
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 0);
motorSet(MOTOR_ARM_LEFT_BACK, -10);
motorSet(MOTOR_ARM_LEFT_FRONT, -10);
delay(55);
motorSet(MOTOR_DRIVE_RIGHT_BACK, 0);
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 0);
motorSet(MOTOR_ARM_LEFT_BACK, 0);
motorSet(MOTOR_ARM_LEFT_FRONT, 0);
delay(200);
}
void scrapeLeft(int x) {
int counts = 0;
imeReset(0); // rest rightLine I.M.E
imeGet(0, &counts);
while (abs(counts) < x) {
motorSet(MOTOR_DRIVE_RIGHT_BACK, 90);
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 90);
motorSet(MOTOR_ARM_LEFT_BACK, 0);
motorSet(MOTOR_ARM_LEFT_FRONT, 0);
imeGet(0, &counts); // keep getting the value
}
motorSet(MOTOR_DRIVE_RIGHT_BACK, -10); // no intertia
motorSet(MOTOR_DRIVE_RIGHT_FRONT, -10);
motorSet(MOTOR_ARM_LEFT_BACK, 0);
motorSet(MOTOR_ARM_LEFT_FRONT, 0);
delay(55);
motorSet(MOTOR_DRIVE_RIGHT_BACK, 0);
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 0);
motorSet(MOTOR_ARM_LEFT_BACK, 0);
motorSet(MOTOR_ARM_LEFT_FRONT, 0);
delay(200);
}
void scrapeLeftBack(int x) {
int counts = 0;
imeReset(0); // rest left I.M.E
imeGet(0, &counts);
while (abs(counts) < x) {
motorSet(MOTOR_DRIVE_RIGHT_BACK, -90);
motorSet(MOTOR_DRIVE_RIGHT_FRONT, -90);
motorSet(MOTOR_ARM_LEFT_BACK, 0);
motorSet(MOTOR_ARM_LEFT_FRONT, 0);
imeGet(0, &counts); // keep getting the value
}
motorSet(MOTOR_DRIVE_RIGHT_BACK, 10); // no intertia
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 10);
motorSet(MOTOR_ARM_LEFT_BACK, 0);
motorSet(MOTOR_ARM_LEFT_FRONT, 0);
delay(55);
motorSet(MOTOR_DRIVE_RIGHT_BACK, 0);
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 0);
motorSet(MOTOR_ARM_LEFT_BACK, 0);
motorSet(MOTOR_ARM_LEFT_FRONT, 0);
delay(200);
}
void scrapeRightBack(int x) {
int counts = 0;
imeReset(1); // rest rightLine I.M.E
imeGet(1, &counts);
while (abs(counts) < x) {
motorSet(MOTOR_DRIVE_RIGHT_BACK, 0);
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 0);
motorSet(MOTOR_ARM_LEFT_BACK, -90);
motorSet(MOTOR_ARM_LEFT_FRONT, -90);
imeGet(1, &counts); // keep getting the value
}
motorSet(MOTOR_DRIVE_RIGHT_BACK, 0); // no intertia
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 0);
motorSet(MOTOR_ARM_LEFT_BACK, 10);
motorSet(MOTOR_ARM_LEFT_FRONT, 10);
delay(55);
motorSet(MOTOR_DRIVE_RIGHT_BACK, 0);
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 0);
motorSet(MOTOR_ARM_LEFT_BACK, 0);
motorSet(MOTOR_ARM_LEFT_FRONT, 0);
delay(200);
}
void forwardDetect() {
int white = 1300;
int leftLine = analogRead(1);
int midLine = analogRead(2);
int rightLine = analogRead(3);
while (midLine > white) // drive till hit white line
{
motorSet(MOTOR_DRIVE_RIGHT_BACK, 60);
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 60);
motorSet(MOTOR_ARM_LEFT_BACK, 60);
motorSet(MOTOR_ARM_LEFT_FRONT, 60);
midLine = analogRead(2);
}
motorSet(MOTOR_DRIVE_RIGHT_BACK, -10); // no intertia
motorSet(MOTOR_DRIVE_RIGHT_FRONT, -10);
motorSet(MOTOR_ARM_LEFT_BACK, -10);
motorSet(MOTOR_ARM_LEFT_FRONT, -10);
delay(55);
motorSet(MOTOR_DRIVE_RIGHT_BACK, 0);
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 0);
motorSet(MOTOR_ARM_LEFT_BACK, 0);
motorSet(MOTOR_ARM_LEFT_FRONT, 0);
delay(200);
}
void backDetect() {
int white = 2000;
int leftLine = analogRead(1);
int midLine = analogRead(2);
int rightLine = analogRead(3);
while (midLine > white) // drive till hit white line
{
motorSet(MOTOR_DRIVE_RIGHT_BACK, -60);
motorSet(MOTOR_DRIVE_RIGHT_FRONT, -60);
motorSet(MOTOR_ARM_LEFT_BACK, -60);
motorSet(MOTOR_ARM_LEFT_FRONT, -60);
midLine = analogRead(2);
}
motorSet(MOTOR_DRIVE_RIGHT_BACK, 10); // no intertia
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 10);
motorSet(MOTOR_ARM_LEFT_BACK, 10);
motorSet(MOTOR_ARM_LEFT_FRONT, 10);
delay(55);
motorSet(MOTOR_DRIVE_RIGHT_BACK, 0);
motorSet(MOTOR_DRIVE_RIGHT_FRONT, 0);
motorSet(MOTOR_ARM_LEFT_BACK, 0);
motorSet(MOTOR_ARM_LEFT_FRONT, 0);
delay(200);
}
*/
void armUp(int x) {
int pot = encoderGet(encoder2);
while (pot < x) {
motorSet(MOTOR_ARM_RIGHT_TOP, -127);
motorSet(MOTOR_ARM_LEFT_TOP, 127);
motorSet(MOTOR_ARM_RIGHT_BOTTOM, -127);
motorSet(MOTOR_ARM_LEFT_BOTTOM, 127);
motorSet(MOTOR_ARM_RIGHT_MID, -127);
motorSet(MOTOR_ARM_LEFT_MID, 127);
pot = encoderGet(encoder2);
}
stopArm();
//armUpTrim();
delay(300);
}
void armUp(int x)
{
int pot = analogRead (8);
while (pot < x)
{
motorSet (motor3, -127) ; // arm up
motorSet (motor4, 127) ;
motorSet (motor7, -127) ;
motorSet (motor8, 127) ;
pot = analogRead (8);
}
stopArm();
delay (1000);
}
void armUpEnc(int x)
{
int towerCount = encoderGet(TOWER_ENCODER);//documentation claims 360 ticks in 1 revolution
printf("Height stop: %d > %d \n\r ",towerCount,x);
while (towerCount > x)
{
motorSet(ARM_MOTOR3, MOTOR_MAX); // arm up
motorSet(ARM_MOTOR4, MOTOR_MAX);
motorSet(ARM_MOTOR7, MOTOR_MAX);
motorSet(ARM_MOTOR8, MOTOR_MAX);
towerCount = encoderGet(TOWER_ENCODER);
}
stopArm();
armUpTrim();
delay (300);
}
///Move to loading height
void armHelixDown()
{
printf("Helix Down\r\n");
motorSet (SWING_MOTOR, -SWING_MOTOR_SPEED);
armDownDead(); // power all lift motors to max
//Swingdone is true for testing, set it back for production
bool done =false, swingDone =true, heightDone =false;
int towerCount=0, swingCount=0;
while(!done)
{
swingCount = analogRead (SWING_POT_PIN); //4095 is 5 volts
towerCount = encoderGet(TOWER_ENCODER);//documentation claims 360 ticks in 1 revolution
//Should not be quite zero
int threshold = SPIKE_OFFSET*0.01;
printf("Height stop: %d < %d \n\r ",towerCount,threshold);
if (towerCount < threshold) //1.25 is an estimate for the load height
{
stopArm();
printf("Tower lowered to load position\n\r");
heightDone = true;
#if DEBUG
delay(5000);
#endif
}
// if (swingCount < 100) //TODo no measurements for pot yet
// {
// motorSet (SWING_POT_PIN, 0);
// printf("Arm swung, may be wrong angle\n");
// swingDone=true;
// }
done = swingDone && heightDone;
}
}
void armHelixUpEnc (int enc, int potTargetValue)
{
//moved to init once
//const Encoder TOWER_ENCODER = encoderInit (2,3,false);
//TOWER_ENCODER = encoderInit (TOWER_ENCODER_TOP_CHANNEL,TOWER_ENCODER_BOTTOM_CHANNEL,false);
armUpDead(); // power all lift motors to max
motorSet (SWING_MOTOR, SWING_MOTOR_SPEED);
//TODO hack, set wingDone true
bool done =false, swingDone =true, heightDone =false;
int towerCount=0, swingCount=0;
printf("Arm Helix up \n\r");
int threshold =enc;
while(!done)
{
swingCount = analogRead (SWING_POT_PIN); //4095 is 5 volts
towerCount = encoderGet(TOWER_ENCODER);//documentation claims 360 ticks in 1 revolution
printf("Height stop: %d > %d \n\r ",towerCount,threshold);
if (towerCount > threshold)
{
stopArm();
printf("Tower raised, might fall, motor stopped\n\r");
heightDone = true;
}
// if (swingCount > potTargetValue)
// {
// motorSet (SWING_MOTOR, 0);
// printf("Arm swung, may be wrong angle\n");
// swingDone=true;
// }
done = swingDone && heightDone;
}
#if DEBUG
delay(5000);
#endif
}
void lowerArm()
{
// Arm lowers
// Speed decreases as we near the bottom.
if(getArmPotentiometer() > Arm_SlowedHeight)
{
motor[rightArm1] = -Arm_DownSpeed;
motor[rightArm2] = -Arm_DownSpeed;
motor[leftArm1] = -Arm_DownSpeed;
motor[leftArm2] = -Arm_DownSpeed;
}
else if(getArmPotentiometer() > Arm_MinHeight)
{
motor[rightArm1] = -Arm_SlowedSpeed;
motor[rightArm2] = -Arm_SlowedSpeed;
motor[leftArm1] = -Arm_SlowedSpeed;
motor[leftArm2] = -Arm_SlowedSpeed;
}
else
{
stopArm();
}
}
void raiseArm()
{
int error = getArmSeperation();
if(getArmPotentiometer() < Arm_MaxHeight)
{
if(abs(error) > 0)
{
if(error > 0) // Left arm is higher than the right arm
{
motor[rightArm1] = Arm_NormalSpeed;
motor[rightArm2] = Arm_NormalSpeed;
motor[leftArm1] = Arm_NormalSpeed - 20;
motor[leftArm2] = Arm_NormalSpeed - 20;
}
else if(error < 0) // Right arm is higher than the left arm
{
motor[rightArm1] = Arm_NormalSpeed - 20;
motor[rightArm2] = Arm_NormalSpeed - 20;
motor[leftArm1] = Arm_NormalSpeed;
motor[leftArm2] = Arm_NormalSpeed;
}
}
else // The difference between arm heights is negligible
{
motor[rightArm1] = Arm_NormalSpeed;
motor[rightArm2] = Arm_NormalSpeed;
motor[leftArm1] = Arm_NormalSpeed;
motor[leftArm2] = Arm_NormalSpeed;
}
}
else
{
stopArm();
}
}
void kakitRed()
{
// variables
int armMin = 300;
int wallHeight = 1000;
int goalHeight = 1650;
int pot = analogRead(8);
int encoder00 = 250; // back up abit to row
int encoder0 = 950; // turn 360 degrees, knock off buckys, face line
int encoder1 = 162; // rotate towards 2 buckys
int encoder2 = 150; // drive towards buckys
int encoder3 = 400; // back up to bump
int encoder4 = 200; // back up towards bridge
int encoder5 = 360; // rotate 180 degrees to the large ball
int encoder6 = 900; // go under bridge and knock out large ball
int encoder7 = 90; // turn to goal
int encoder8 = 200; // drive to goal
int encoder9 = 75;
// begin routine
intake(300);
armDownTrim();
driveForwardDead(); //ram big balls
delay(1500);
stopDrive();
delay(500);
driveBack(encoder00); // back up to row abit
intakeDead();
turnLeft(encoder0); // turn 360 degrees
driveBackDead(); // drive back + wall align
delay(1300);
stopDrive();
delay(500);
turnRight(encoder1); // turn to two buckys
intakeDead();
followLine(300); // make sure ur straight
driveForwardSlowDead(); // drive towards buckys
delay(500);
stopDrive();
delay(600);
driveForwardDead(); //get the 2nd ball
delay(200);
stopDrive();
delay(600);
stopIntake();
driveBack(encoder3); // back up to bump
armUpDead(); // armup
delay(50);
stopArm();
stopIntake();
driveBackSlowDead(); // allign the bump
delay(300);
stopDrive();
delay(500);
driveBackDead(); // over the bump
delay(1000);
stopDrive();
delay(500);
driveForwardSlowDead(); // alighn to bump
delay(800);
stopDrive();
delay(500);
driveBackSlow(encoder4); // back up towards bridge
turnLeft(encoder5); // rotate 180 degrees to the large ball
armDown(armMin);
armDownTrim();
driveForward(encoder6); // go under the bridge + knock large ball
armUp(goalHeight);
turnRight(encoder7); // turn to goal
findLineRight();
followLine(300);
driveForwardSlowDead(); // drive to goal
delay(700);
stopDrive();
outtake(7000); // outtake 3
stopAll();
}
task usercontrol()
{
while(true)
{
driveWithJoysticks();
intakeControl();
if(vexRT[Btn6U]== 1 && vexRT[Btn6D] == 0 && vexRT[Btn8U]== 0 && vexRT[Btn8D]== 0)
{
raiseArm();
}
else if(vexRT[Btn6U] == 0 && vexRT[Btn6D] == 1 && vexRT[Btn8U] == 0 && vexRT[Btn8D] == 0 && vexRT[Btn8R] == 0)
{
lowerArm();
}
else if(vexRT[Btn6U] == 0 && vexRT[Btn6D] == 0 && vexRT[Btn8U] == 1 && vexRT[Btn8D] == 0 && vexRT[Btn8R] == 0)
{
positionArm(Arm_ScoreHeight);
}
else if(vexRT[Btn6U] == 0 && vexRT[Btn6D] == 0 && vexRT[Btn8U] == 0 && vexRT[Btn8D] == 1 && vexRT[Btn8R] == 0)
{
positionArm(Arm_MinHeight);
}
else
{
stopArm();
}
if(vexRT[Btn7U] == 1)
{
if(btn7UPressed != 1)
{
currentIntakeSpeedStep++;
if(currentIntakeSpeedStep > 2)
{
currentIntakeSpeedStep = 2;
}
btn7DPressed = true;
}
}
else
{
btn7UPressed = false;
}
if(vexRT[Btn7D] == 1)
{
if(btn7DPressed != 1)
{
currentIntakeSpeedStep--;
if(currentIntakeSpeedStep < 1)
{
currentIntakeSpeedStep = 1;
}
btn7DPressed = true;
}
}
else
{
btn7DPressed = false;
}
switch(currentIntakeSpeedStep)
{
case 1:
currentIntakeSpeed = IntakeSlow;
break;
case 2:
currentIntakeSpeed = IntakeFast;
break;
default:
currentIntakeSpeed = IntakeFast;
break;
}
}
while ((vexRT[Btn7D] == 1) || (vexRT[Btn7U] == 1) || (vexRT[Btn7R] == 1) || (vexRT[Btn7L] == 1))
{
// Wait until button is released to go back to single joystick control.
driveWithJoysticks();
}
}
task autonomous()
{
switch(selectedAuton)
{
case Autonomous_LeftScoreMatchLoads:
{
clearEncoders();
motor[intakeTread] = -127;
motor[intakeWheel] = -127;
wait1Msec(700);
stopIntake();
positionArm(Arm_ScoreHeight);
stopArm();
wait1Msec(1000);
int leftDest = SensorValue[encoder_Left] + 2200;
while(SensorValue[encoder_Left] < leftDest)
{
driveAtSpeed(60);
}
stopDrive();
motor[intakeTread] = 127;
motor[intakeWheel] = 127;
wait10Msec(200);
}
break;
case Autonomous_RightPickupScoreYellow:
{
clearEncoders();
int leftDest = SensorValue[encoder_Left] + 1850;
while(SensorValue[encoder_Left] < leftDest)
{
driveAtSpeed(50);
}
stopDrive();
motor[intakeTread] = -127;
motor[intakeWheel] = -127;
wait1Msec(1000);
stopIntake();
positionArm(Arm_ScoreHeight);
int rightDest = SensorValue[encoder_Right] - 730;
while(SensorValue[encoder_Right] > rightDest)
{
motor[driveLeftBack] = 38;
motor[driveRightBack] = 0;
motor[driveLeftFront] = 38;
motor[driveRightFront] = 0;
}
leftDest = SensorValue[encoder_Left] + 650;
while(SensorValue[encoder_Left] < leftDest)
{
motor[driveLeftBack] = 38;
motor[driveRightBack] = 40;
motor[driveLeftFront] = 38;
motor[driveRightFront] = 40;
}
stopDrive();
motor[intakeTread] = 127;
motor[intakeWheel] = 127;
wait1Msec(1500);
stopIntake();
}
break;
case Autonomous_ProgrammingSkills:
{
clearEncoders();
int leftDest = SensorValue[encoder_Left] + 1850;
while(SensorValue[encoder_Left] < leftDest)
{
driveAtSpeed(50);
}
stopDrive();
motor[intakeTread] = -127;
motor[intakeWheel] = -127;
wait1Msec(1000);
stopIntake();
positionArm(Arm_ScoreHeight);
int rightDest = SensorValue[encoder_Right] - 730;
while(SensorValue[encoder_Right] > rightDest)
{
motor[driveLeftBack] = 38;
motor[driveRightBack] = 0;
motor[driveLeftFront] = 38;
motor[driveRightFront] = 0;
}
leftDest = SensorValue[encoder_Left] + 650;
while(SensorValue[encoder_Left] < leftDest)
{
driveAtSpeed(40);
}
stopDrive();
motor[intakeTread] = 127;
motor[intakeWheel] = 127;
wait1Msec(500);
stopIntake();
leftDest = SensorValue[encoder_Left] - 390;
while(SensorValue[encoder_Left] > leftDest)
{
driveAtSpeed(-30);
}
stopDrive();
positionArm(990);
stopArm();
rightDest = SensorValue[encoder_Right] + 660;
while(SensorValue[encoder_Right] < rightDest)
{
motor[driveLeftBack] = -30;
motor[driveLeftFront] = -30;
}
stopDrive();
leftDest = SensorValue[encoder_Left] - 2050;
while(SensorValue[encoder_Left] > leftDest)
{
driveAtSpeed(-50);
}
stopDrive();
}
break;
case Autonomous_None:
default:
{
}
break;
}
}
