void reset_op()
{
encoderReset(rf_encoder);
encoderReset(lf_encoder);
encoderReset(rb_encoder);
encoderReset(lb_encoder);
}
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);
}
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);
}
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 armbotRed()
{
encoderReset(TOWER_ENCODER);
printf("ENCODER %x reset\n\r",TOWER_ENCODER);
int stackRot = 3360;
int loadRot = 100;
int loadHeight = 70;
int loadHeightHigh = 376 +30; //30 is error
int wallHeightU = loadHeightHigh; // idk
int wallHeight = loadHeight; // idk
int spike1 = -6; // heights for the individual spikes!
int spike2 = 450; //n*(offset+ drift)
//150 = 450-300 release height
int spike3 = 800;
int spike4 = 0;
int spike5 = 0;
int spike6 = 0;
int pot = analogRead (8);
int wall = 980; //pot value
int stack = 3360; // pot value
int center = (stack-wall)/2;
/////////////////////////////////////spike 1//////////////////////////////////////////
armUpEnc(wallHeight);
turnRightSlow(wall);
intake(300); //clamp it!
armUpEnc(wallHeightU); // pick it up
turnLeftSlow(stack); // rotate to stack
armDownEnc(spike1); //score the spike!
outtake(300);
armUpEnc(spike1 + 500);
turnRightSlow(center);
// and loop!/////////////////////spike 2//////////////////////////
armUpEnc(wallHeightU);
turnRightSlow(wall);
intake(300); //clamp it!
armUpEnc(spike2 + 100); // pick it up + go abit higher than spike height!
turnLeftSlow(stack); // rotate to stack
delay(500); // wait for swing to stop!
armDownEnc(spike1); //score the spike!
outtake(300);
armUpEnc(spike2 + 500);
turnRightSlow(center);
//end of routine
stopAll () ;
delay(60000);///////////////////////////////////////////////////////////////////////////////////
}
void test_init() {
TEST_ASSERT_EQUAL(0, encoderState(encoder));
TEST_ASSERT_EQUAL(0, getEncoderError(encoder).errors);
TEST_ASSERT_EQUAL(0, getEncoderError(encoder).errorMask);
encoderReset(encoder, 100);
TEST_ASSERT_EQUAL(100, encoderState(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() {
encoderReset(encoder);
while (1) {
int distance = encoderGet(encoder);
printf("%d\n\r", distance);
delay(20);
}
}
void flywheelReset(Flywheel *flywheel)
{
flywheel->derivative = 0.0f;
flywheel->integral = 0.0f;
flywheel->error = 0.0f;
flywheel->action = 0.0f;
flywheel->lastAction = 0.0f;
flywheel->lastError = 0.0f;
flywheel->firstCross = true;
flywheel->reading = 0;
encoderReset(flywheel->encoder);
}
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 calibrate(){
if (bot == calib){
wind(joystickGetAnalog(1, 3));
if (joystickGetDigital(1, 7, JOY_RIGHT)){
encoderReset(encode);
}
}
int oldval = leftdraw;
//raise the left shooting value
while (joystickGetDigital(1, 7, JOY_UP)){
leftdraw = oldval - 5;
delay(25);
}
//lower left shooting value
while (joystickGetDigital(1, 7, JOY_DOWN)){
leftdraw = oldval + 5;
delay(25);
}
oldval = rightdraw;
//raise right shooting value
while (joystickGetDigital(1, 8, JOY_UP)){
rightdraw = oldval + 5;
delay(25);
}
//lower left shooting value
while (joystickGetDigital(1, 8, JOY_DOWN)){
rightdraw = oldval - 5;
delay(25);
}
char buf[16];
sprintf(buf, "R: %d L: %d", rightdraw, leftdraw);
lcdSetText(uart1, 2, buf);
}
void startSensor(Sensor *s) {
switch (s->type) {
case SHAFT_ENCODER:
encoderReset(s->enc);
if (DEBUG) {
print("reset encoder");
}
break;
case GYROSCOPE:
gyroReset(s->gyr);
break;
case TIME:
//resetTimer(s->port, true);
startTimer(s->port, true);
break;
case IME:
imeReset(s->port);
break;
default:
break;
}
}
void quadEncoderReset(QuadEncoder encoder) {
encoderReset(encoder.encoder_data);
}
/*
* Runs the user autonomous 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 autonomous mode. If the robot is disabled or communications is
* lost, the autonomous task will be stopped by the kernel. Re-enabling the robot will restart
* the task, not re-start it from where it leftLine off.
*
* Code running in the autonomous task cannot access information from the VEX Joystick. However,
* the autonomous function can be invoked from another task if a VEX Competition Switch is not
* available, and it can access joystick information if called in this way.
*
* The autonomous task may exit, unlike operatorControl() which should never exit. If it does
* so, the robot will await a switch to another mode or disable/enable cycle.
*/
void autonomous()
{
// jumper in = 1
int jumper1 = digitalRead (1);
int jumper2 = digitalRead (2);
int jumper3 = digitalRead (3);
encoderReset(TOWER_ENCODER);
if (jumper2 == 1) // this is gordons last sec brute force method cuz theres no time //jumper 2 in
{// jumper 2 in
armbotRed();
}
AutonomousMode mode = (jumper3) | (jumper2 << 1) | (jumper1 <<2);
printf("Entered autonomous mode %d\n\r",mode);
encoderReset(TOWER_ENCODER);
printf("ENCODER %x reset\n\r",TOWER_ENCODER);
//jumper 1 = MSB // jumper 3 = LSB
armbotRed();
//armTest();
/*
switch(mode)
{
case BLUE_RESERVED0: // 0 0 0
armbotRed();;
break;
case BLUE_RESERVED1: // 0 0 1
stackEm();
break;
case BLUE_RESERVED2: // 0 1 0
stackEm();
break;
case BLUE_RESERVED3: // 0 1 0
//mode3()
break;
case RED_RESERVED4: // 1 0 0
stackEm();
break;
case RED_RESERVED5: // 1 0 1
stackEm();
break;
case RED_RESERVED6: // 1 1 0
stackEm();
break;
default:
printf("Jumpers are set to unkown configuration\n\r");
stackEm();
break;
}
*/
// all jumper comands here:
//1 = out
// 0 = in
/*
if ((jumper1 == 1) ) // jumper in 1 = red autonomous
{
armbotRed();
}
if (jumper2 == 0) // jumper 2 in = blue auto
{
//rejectionBlue();
}
*/
//allstop
stopAll();
delay(600000);
}
/*
* 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() {
//Config variables 'n stuff
bool autoM = false;
bool first = true;
bool reset = true;
const int LFRONT=1;
const int LBACK=2;
const int RFRONT=3;
const int RBACK=4;
const int WINCH1=5;
const int WINCH2=6;
const int FLAGDWN=7;
long startM;
long endM;
double circ;
double MPH;
//main loop
while (1) {
//motorSet(2,50);
//auto code
autoM = joystickGetDigital(1, 8, JOY_LEFT);
if (autoM && first) {
autonomous();
first = false;
}
//delay(20);
//Teleop code
if (joystickGetDigital(1, 7, JOY_UP)) { //Drive straight
int speed = joystickGetAnalog(1, 3);
motorSet(LFRONT, speed);
//delay(5);
motorSet(LBACK, speed);
//delay(5);
motorSet(RFRONT, speed);
//delay(5);
motorSet(RBACK, speed);
//delay(5);
} else { //tank drive
int LSpeed = joystickGetAnalog(1, 3);
int RSpeed = joystickGetAnalog(1, 2);
motorSet(LFRONT, LSpeed);
motorSet(LBACK, LSpeed);
motorSet(RFRONT, RSpeed);
motorSet(RBACK, RSpeed);
}
//Winch 1
if (joystickGetDigital(1, 5, JOY_UP)) {
motorSet(WINCH1, 60);
} else if (joystickGetDigital(1, 5, JOY_DOWN)) {
motorSet(WINCH1, -60);
}
else motorSet(WINCH1, 0);
//Winch 2
if (joystickGetDigital(1, 6, JOY_UP)) {
motorSet(WINCH2, 60);
} else if (joystickGetDigital(1, 6, JOY_DOWN)) {
motorSet(WINCH2, -60);
}
else motorSet(WINCH2, 0);
//Flag
if (joystickGetDigital(1, 8, JOY_DOWN)) {
motorSet(FLAGDWN, 60);
}
else if (joystickGetDigital(1,8, JOY_UP)){
motorSet(FLAGDWN, -60);
}
else motorSet(FLAGDWN, 0);
//Anemometer
if(reset){
startM=micros();
encoderReset(ameter);
reset=false;
}
long reading = encoderGet(ameter);
if(reading/360==4){
endM=micros();
double revPerSec = (4.0*pow(10.0, 6.0))/((double)(startM-endM));
double revPerMin = revPerSec*60.0;
MPH = circ/12.0/5280.0*revPerMin*60.0;
reset=true;
}
//Flush Data
delay(20);
}
}
