int gofor(int llen, int rlen, int turn, int forward) {
encoderReset(r_encoder);
encoderReset(l_encoder);
int ol=0;
int or=0;
dstat=0;
printf("going for %d\t%d degrees with dir %d\t%d\n\r",llen,rlen,turn,forward);
#ifndef SIM
while((abs(encoderGet(l_encoder))<llen &&abs(encoderGet(r_encoder))<rlen)){
c_status("gofor",llen);
controldrive(turn,forward);
simtank(<ank,encoderGet(l_encoder-ol)-ol,encoderGet(r_encoder)-or);
ol=encoderGet(l_encoder);
or=encoderGet(r_encoder);
delay(20);
}
#endif
dstat=1;
controldrive(0,0);
printf("Motion complete, expected:");
printpos(&ctank);
printf("\tintegrated: ");
printpos(<ank);
printf("\n\r");
return encoderGet(l_encoder)>encoderGet(l_encoder) ? encoderGet(l_encoder) : encoderGet(r_encoder);
}
double getRedEncoderVelocity(RedEncoder *encoder)
{
if((millis() - (*encoder).lastTime) > (*encoder).sampleTime)
{
double dPosition = (double) (encoderGet((*encoder).encoder) -
(*encoder).lastReading);
int dTime = (int) (millis() - (*encoder).lastTime);
double velocity = dPosition / dTime;
(*encoder).lastReading = encoderGet((*encoder).encoder);
(*encoder).lastTime = millis();
(*encoder).lastVelocity = velocity;
puts("Update");
return velocity;
}
else
{
puts("No");
return (*encoder).lastVelocity;
}
}
void c_status(char *message, int n) {
static int callcount=0;
if(callcount >10) {
printf("STATUS: DIST:%d\t%d\t%s %d\n\r",encoderGet(l_encoder),encoderGet(r_encoder),message,n);
callcount=0;
}
callcount=callcount+1;
}
void lcdSensor(){// code for the sensor sub menu and displays in the lcd
if ((lcdReadButtons(uart1) == LCD_BTN_CENTER && previousLCD == LCD_BTN_CENTER) || menuStay == LCD_BTN_CENTER){
lcdClear(uart1);
arbitraryVariable = 1;
menuStay = LCD_BTN_CENTER;
// sensor menu!!
if (lcdReadButtons(uart1) == LCD_BTN_LEFT && previousLCD != LCD_BTN_LEFT){// if clicked now, and not clicked 100 ms before
sensorMenuCount--;
}
else if (lcdReadButtons(uart1) == LCD_BTN_RIGHT && previousLCD != LCD_BTN_RIGHT){
sensorMenuCount++;
}
if (sensorMenuCount < 0){
sensorMenuCount = sensorMaxNumberMenus;
}
else if (sensorMenuCount > sensorMaxNumberMenus){
sensorMenuCount = 0;
}
switch (sensorMenuCount) {
case 0:
lcdClear(uart1);
lcdSetText(uart1, 1, "Encoder Average:");
snprintf(encoderAve, 17, "%d", ((encoderGet(frontLeftEncoder) + encoderGet(frontRightEncoder) +encoderGet(backLeftEncoder) +encoderGet(backRightEncoder))/numbOfEncod));
lcdSetText(uart1, 2, encoderAve);
if (lcdReadButtons(uart1) == LCD_BTN_CENTER && previousLCD != LCD_BTN_CENTER){
menuCount = 0;
menuStay = 0;
arbitraryVariable = 0;
endResultMenu = 0;
}
break;
case 1:
lcdClear(uart1);
lcdSetText(uart1, 1, "Lift Pot:");
snprintf(liftPotLCD, 17, "%d", (analogRead(lift_pot)));
lcdSetText(uart1, 2, liftPotLCD);
if (lcdReadButtons(uart1) == LCD_BTN_CENTER && previousLCD != LCD_BTN_CENTER){
menuCount = 0;
menuStay = 0;
arbitraryVariable = 0;
endResultMenu = 0;
}
break;
case 2:
lcdClear(uart1);
lcdSetText(uart1, 1, "Pincer Pot Ave:");
snprintf(pincePotLCD, 17, "%d", (analogRead(pince_pot_left) + analogRead(pince_pot_right))/2);
lcdSetText(uart1, 2, pincePotLCD);
if (lcdReadButtons(uart1) == LCD_BTN_CENTER && previousLCD != LCD_BTN_CENTER){
menuCount = 0;
menuStay = 0;
arbitraryVariable = 0;
endResultMenu = 0;
}
break;
}
}
}
int get_foreward()
{
int forward_lf = 1 * encoderGet(lf_encoder);
int forward_lb = 1 * encoderGet(lb_encoder);
int forward_rf = -1 * encoderGet(rf_encoder);
int forward_rb = -1 * encoderGet(rb_encoder);
int result = (forward_lf + forward_lb + forward_rf + forward_rb) / 4;
printf("%8d\n\r",result);
return result;
}
void turnLeft(int encoder_value){
while(((-1*encoderGet(frontLeftEncoder)) + encoderGet(frontRightEncoder))/2 > encoder_value){
motorSet(front_left_drive, 127);
motorSet(front_right_drive, 127);
motorSet(back_left_drive, 127);
motorSet(back_right_drive, - 127);
}
encoderReset(frontLeftEncoder);
encoderReset(frontRightEncoder);
}
void driveForward(int encoder_value){
while((encoderGet(frontLeftEncoder)+ encoderGet(frontRightEncoder))/2 < encoder_value){
motorSet(front_left_drive, -127);
motorSet(front_right_drive, 127);
motorSet(back_left_drive, -127);
motorSet(back_right_drive, -127);
}
encoderReset(frontLeftEncoder);
encoderReset(frontRightEncoder);
}
int get_turn()
{
int turn_lf = 1 * encoderGet(lf_encoder);
int turn_lb = 1 * encoderGet(lb_encoder);
int turn_rf = 1 * encoderGet(rf_encoder);
int turn_rb = 1 * encoderGet(rb_encoder);
//return (turn_lf + turn_lb + turn_rf + turn_rb) / 4;
int result = (turn_lf + turn_lb + turn_rf + turn_rb) / 4;
printf("%8d\n\r",result);
return result;
}
int encoderValue(){
/** The value of the left base encoder */
int l=encoderGet(encoderBaseLeft);
/** The value of the right base encoder */
int r=encoderGet(encoderBaseRight);
/** The weighted average value of the base encoders */
float lw = 1.0f;//left weighting
float rw = 1.0f;//right weighting
float tot = 1.0f;//totat weighting
int chP = FPSBase.direction.chY;
int chS = FPSBase.direction.chX;
if (abs(chP) > 0 || abs(chS) > 0)
{
if (sgnSimple(chS) == sgnSimple(chP))
{
lw = sgnSimple(chP) * sqrt((float)(chS * chS + chP * chP));
rw = chP - chS;
//lw = sqrt((float)(chS * chS + chP * chP));
//rw = abs(chP - chS);
}
else
{
lw = chP + chS;
rw = sgnSimple(chP) * sqrt((float)(chS * chS + chP * chP));
//lw = abs(chP + chS);
//rw = sqrt((float)(chS * chS + chP * chP));
}
lw = abs(lw);
rw = abs(rw);
tot = lw + rw;
//tot = lw + rw;
}
lw /= tot;
rw /= tot;
printf("l:%5d, r:%5d", l, r);
printf("lw:%4f, rw:%4f", lw, rw);
printf("l*:%4f, r*:%4f", (l * lw), (r * rw));
//return (l+r)/2;
return (int)(l * lw + r * rw);
}
int get_sideways()
{
//*
int sideways_lf = 1 * encoderGet(lf_encoder);
int sideways_lb = -1 * encoderGet(lb_encoder);
int sideways_rf = 1 * encoderGet(rf_encoder);
int sideways_rb = -1 * encoderGet(rb_encoder);
//return (sideways_lf + sideways_lb + sideways_rf + sideways_rb) / 4;
int result = (sideways_lf + sideways_lb + sideways_rf + sideways_rb) / 4;
printf("%8d\n\r",result);
return result;
//*/
//return encoderGet(lf_encoder);
}
/*
* Runs the user operator control code. This function will be started in its own task with the
* default priority and stack size whenever the robot is enabled via the Field Management System
* or the VEX Competition Switch in the operator control mode. If the robot is disabled or
* communications is lost, the operator control task will be stopped by the kernel. Re-enabling
* the robot will restart the task, not resume it from where it left off.
*
* If no VEX Competition Switch or Field Management system is plugged in, the VEX Cortex will
* run the operator control task. Be warned that this will also occur if the VEX Cortex is
* tethered directly to a computer via the USB A to A cable without any VEX Joystick attached.
*
* Code running in this task can take almost any action, as the VEX Joystick is available and
* the scheduler is operational. However, proper use of delay() or taskDelayUntil() is highly
* recommended to give other tasks (including system tasks such as updating LCDs) time to run.
*
* This task should never exit; it should end with some kind of infinite loop, even if empty.
*/
void operatorControl() {
int left = 0;
int right = 0;
while (1) {
arcadeDrive(drive, OIGetDriveMagnitude(), OIGetDriveRotation());
delay(20);
left = encoderGet(drive.leftEncoder);
right = encoderGet(drive.rightEncoder);
printf("Left: %d\nRight: %d\n\n", left, right);
}
}
void armDownEnc(int x)
{
int counts = encoderGet(encoder);
while (abs(counts) < x)
{
motorSet (motor3, 127) ; // arm down
motorSet (motor4, -127) ;
motorSet (motor7, 127) ;
motorSet (motor8, -127) ;
counts = encoderGet(encoder);//keep getting the value
}
armDownTrim();
delay (300);
}
int quadEncoderGet(QuadEncoder encoder) {
int value = encoderGet(encoder.encoder_data);
if (encoder.inverted) {
value = -value;
}
return value;
}
/*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 armDownEnc(int x)
{
int counts = encoderGet(encoder);
while (abs(counts) < x)
{
motorSet(ARM_MOTOR1, MOTOR_MAX); // arm down
motorSet(ARM_MOTOR2, -MOTOR_MAX);
motorSet(ARM_MOTOR3, MOTOR_MAX);
motorSet(ARM_MOTOR4, -MOTOR_MAX);
counts = encoderGet(encoder);//keep getting the value
}
armDownTrim();
delay (300);
}
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;
}
}
RedEncoder initRedEncoder(Encoder encoder, unsigned long refreshTime)
{
RedEncoder newEncoder = {encoder, malloc(sizeof(unsigned long)), malloc(sizeof(int)), malloc(sizeof(double)), refreshTime};
*newEncoder.lastReadTime = micros();
*newEncoder.lastEncoder = encoderGet(encoder);
*newEncoder.velocity = 0;
return newEncoder;
}
/*
* Runs the user operator control code. This function will be started in its own task with the
* default priority and stack size whenever the robot is enabled via the Field Management System
* or the VEX Competition Switch in the operator control mode. If the robot is disabled or
* communications is lost, the operator control task will be stopped by the kernel. Re-enabling
* the robot will restart the task, not resume it from where it left off.
*
* If no VEX Competition Switch or Field Management system is plugged in, the VEX Cortex will
* run the operator control task. Be warned that this will also occur if the VEX Cortex is
* tethered directly to a computer via the USB A to A cable without any VEX Joystick attached.
*
* Code running in this task can take almost any action, as the VEX Joystick is available and
* the scheduler is operational. However, proper use of delay() or taskDelayUntil() is highly
* recommended to give other tasks (including system tasks such as updating LCDs) time to run.
*
* This task should never exit; it should end with some kind of infinite loop, even if empty.
*/
void operatorControl() {
encoderReset(encoder);
while (1) {
int distance = encoderGet(encoder);
printf("%d\n\r", distance);
delay(20);
}
}
// Drive an amount of ticks as straight as possible
// Test status:
// completely untested
void pLoopDriveStraightRaw(int tickDiff, bool correctBackwards, bool correctDir,
double pSpeed, double dSpeed, double pCorrect,
int thresh, int threshCount) {
int leftInit = encoderGet(getEncoderBL()); // Initial left value
int rightInit = encoderGet(getEncoderBR()); // Initial right value
int errorL; // Error in the left side
int errorR; // Error in the right side
int error; // Averaged Error
int lastError = 0; // The Error from the Previous Iteration
int speed; // Calculated speed to drive at
int stopCount = 0; // Amount of time spent within threshold
int initGyro = gyroGet(getGyro()); // Initial value of the gyro
int speedModif = 0; // How much to modify the speed for angle
int angleOffset; // How much the robot is curving in its motion
int iterations = 0;
while (iterations++ <= (P_LOOP_DRIVE_BREAK_TIME / 20)) {
errorL = tickDiff - (encoderGet(getEncoderBL()) - leftInit);
errorR = tickDiff - (encoderGet(getEncoderBR()) - rightInit);
error = errorR; // round((errorL + errorR) / 2); // errorL;
speed = error * pSpeed + ((error - lastError) * dSpeed);
speed = (abs(speed) > 127) ? (speed < 0) ? -127 : 127 : speed;
speed = (abs(speed) < 25) ? (speed < 0) ? -25 : 25 : speed;
angleOffset = gyroGet(getGyro()) - initGyro;
speedModif = (correctDir) ? angleOffset * pCorrect : 0;
// Set motors to linearized version of input speeds
driveRaw(linSpeed(speed + speedModif), linSpeed(speed + speedModif),
linSpeed(speed - speedModif), linSpeed(speed - speedModif));
if (abs(error) < thresh) {
stopCount++;
if (stopCount >= threshCount || !correctBackwards)
break;
}
lastError = error;
delay(20);
}
driveRaw(-speed, -speed, -speed, -speed); // Slam the breaks
delay(10);
driveRaw(0, 0, 0, 0);
}
/**
Get total distance traveled in meters since last reset
The diameter param should be in units of meters
*/
float getDistance(Encoder encoder, float diameter) {
// Get distance of one rotation and multiply it by the number of rotations
// the encoder has made
float dis = PI * diameter;
dis *= encoderGet(encoder) / 360;
return dis;
}
RedEncoder initRedEncoder(Encoder encoder, long refreshTime)
{
long *currentTime = malloc(sizeof(long));
int *currentEncoder = malloc(sizeof(int));
double *velocity = malloc(sizeof(double));
*currentTime = micros();
*currentEncoder = encoderGet(encoder);
*velocity = 0;
RedEncoder newEncoder = {encoder, currentTime, currentEncoder, velocity};
return newEncoder;
}
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);
}
double getRedEncoderVelocity(RedEncoder encoder)
{
if((unsigned int) (micros() - (*encoder.lastReadTime)) > (unsigned int) encoder.refreshTime)
{
if(micros() - (*encoder.lastReadTime) < 1000000)
{
int currentEncoder = encoderGet(encoder.encoder);
//lcdPrint(uart1, 2, "%d", currentEncoder - *encoder.lastEncoder);
double velocity = (double) ((double) (currentEncoder - (*encoder.lastEncoder)) /
(double) (micros() - (*encoder.lastReadTime)));
*encoder.lastEncoder = encoderGet(encoder.encoder);
*encoder.lastReadTime = micros();
velocity *= 100000;
//lcdPrint(uart1, 2, "%f", velocity);
*encoder.velocity = velocity;
}
else
{
(*encoder.lastEncoder) = encoderGet(encoder.encoder);
(*encoder.lastReadTime) = micros();
(*encoder.velocity) = 0;
lcdSetText(uart1, 2, "Timeout");
}
}
//lcdPrint(uart1, 1, "C%d", (unsigned int) micros());
//lcdPrint(uart1, 2, "L%d", (unsigned int) micros() - *encoder.lastReadTime);
//lcdPrint(uart1, 2, "%f", *encoder.velocity);
return *encoder.velocity;
}
void display(char* disp){
if (current == driver){
if (announce != NULL && millis() - lastannounce <= 3000){
lcdSetText(uart1, 1, announce);
}
else {
if (bot == calib){
char buf[16];
sprintf(buf, "Calibrate: %d", encoderGet(encode));
lcdSetText(uart1, 1, buf);
} else if (bot == lift){
if (drinv == 1){
lcdSetText(uart1, 1, "Up");
} else {
lcdSetText(uart1, 1, "Down");
}
} else if (bot == shoot){
if (shootdir == left){
char buf[16];
sprintf(buf, "Left: %d", encoderGet(encode));
lcdSetText(uart1, 1, buf);
} else {
char buf[16];
sprintf(buf, "Right: %d", encoderGet(encode));
lcdSetText(uart1, 1, buf);
}
} else if (bot == control){
char buf[16];
sprintf(buf, "Encoder: %d", encoderGet(encode));
lcdSetText(uart1, 1, buf);
}
}
}
else {
lcdSetText(uart1, 1, disp);
}
}
void autonomous() {
Encoder leftEnc; //Sets encoder variable
Encoder rightEnc;
leftEnc = encoderInit(1, 2, 0); //Points to encoders
rightEnc= encoderInit(3, 4, 0);
encoderReset(leftEnc); //Resets Encoder
encoderReset(rightEnc);
int leftVal; //Initializes value variable
int rightVal;
leftVal = encoderGet(leftEnc); //Gets initial value for encoder
rightVal= encoderGet(rightEnc);
motorSet(1, 127); //Motors Forward
motorSet(2, 127);
motorSet(9, -127);
motorSet(10, 127);
while(leftVal <= 360 && rightVal <= 360) {
leftVal = encoderGet(leftEnc); //Gets encoder value
rightVal= encoderGet(rightEnc);
}
motorSet(1, 0); //Stops Motors
motorSet(2, 0);
motorSet(9, 0);
motorSet(10, 0);
}
void startIOTask(void *ignore) {
while(1) {
setDriveTrainMotors(driveTrainStyle);
motorSet(ARMLeft.port, ARMLeft.out * ARMLeft.reflected);
motorSet(ARMRight.port, ARMRight.out * ARMRight.reflected);
motorSet(ARMTopLeft.port, ARMTopLeft.out * ARMTopLeft.reflected);
motorSet(ARMTopRight.port, ARMTopRight.out * ARMTopRight.reflected);
motorSet(ARMBottomLeft.port, ARMBottomLeft.out * ARMBottomLeft.reflected);
motorSet(ARMBottomRight.port, ARMBottomRight.out * ARMBottomRight.reflected);
if(useIMEs) {
imeGet(IMELEFT, &encVals[0]);
imeGet(IMERIGHT, &encVals[1]);
} else {
encVals[0] = encoderGet(encLeft);
encVals[1] = encoderGet(encRight);
}
gyroVal = gyroGet(gyro);
delay(10);
}
}
double getRedEncoderVelocity(RedEncoder encoder)
{
if(micros() - (*encoder.lastReadTime) > 100000)
{
if(micros() - (*encoder.lastReadTime) < 1000000)
{
int currentEncoder = encoderGet(encoder.encoder);
double velocity = (double) ((double) (currentEncoder - (*encoder.lastEncoder)) /
(double) (micros() - (*encoder.lastReadTime)));
*encoder.lastEncoder = encoderGet(encoder.encoder);
*encoder.lastReadTime = micros();
velocity *= 100000;
//lcdPrint(uart1, 1, "%f", velocity);
*encoder.velocity = velocity;
//lcdSetText(uart1, 2, "Update");
}
else
{
(*encoder.lastEncoder) = encoderGet(encoder.encoder);
(*encoder.lastReadTime) = micros();
(*encoder.velocity) = 0;
lcdSetText(uart1, 2, "Timeout");
}
}
//lcdPrint(uart1, 1, "%f", *encoder.velocity);
return *encoder.velocity;
}
///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
}
