Hello! Here is the code we are taking to worlds! (not the final version) Any and all improvements would be great! Sorry that it isn’t organized and commented out. If you have any questions don’t hesitate to ask!
#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, in1, PlightClaw, sensorReflection)
#pragma config(Sensor, dgtl1, PfreeWheelRight, sensorQuadEncoder)
#pragma config(Sensor, dgtl3, PfreeWheelLeft, sensorQuadEncoder)
#pragma config(Sensor, dgtl5, Pbumper, sensorTouch)
#pragma config(Sensor, dgtl6, Pclaw, sensorDigitalOut)
#pragma config(Sensor, dgtl7, PsonarRight, sensorSONAR_raw)
#pragma config(Sensor, I2C_1, PencoderArm, sensorQuadEncoderOnI2CPort, , AutoAssign )
#pragma config(Motor, port2, armLeft, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port3, armRightY, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port4, wheelLeftBack, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port5, wheelLeftFront, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port6, wheelRightBack, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port7, wheelRightFront, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port8, armLeftY, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port9, armRight, tmotorVex393_MC29, openLoop, reversed)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)
#pragma platform(VEX)
#pragma competitionControl(Competition)
#include "Vex_Competition_Include.c"
#define sonarRight SensorValue[PsonarRight]
#define lightClaw SensorValue[PlightClaw]
#define freeWheelRight SensorValue[PfreeWheelRight]
#define freeWheelLeft SensorValue[PfreeWheelLeft]
#define bumper SensorValue[Pbumper]
#define encoderArm SensorValue[PencoderArm]
#define LCD_NONE 0
#define LCD_LEFT 1
#define LCD_MID 2
#define LCD_RIGHT 4
#define resetEncoders freeWheelRight=freeWheelLeft=0
#define CHANNEL_DEAD_BAND 15 //resting channel error at start
#define FOLD_OUT_ARM_UP -200 //how much the arm needs to go up when deploying
#define FOLD_OUT_ARM_DOWN -250 //how much the arm needs to go down when deploying
#define SCORE_HEIGHT -750 //how high the arm needs to go to score
#define LOAD_HEIGHT -225 //how high to store stars
#define THROW_HEIGHT -444 //how high to throw the stars
#define KpStandard -1.3 //how much power is needed to hold up 3 stars
#define USERCONTROL_WAIT_TIME 10 //how much we wait in usercontrol
#define wheels motor[wheelLeftFront]=motor[wheelLeftBack]=motor[wheelRightBack]=motor[wheelRightFront]
#define wheelLeft motor[wheelLeftFront]=motor[wheelLeftBack]
#define wheelRight motor[wheelRightFront]=motor[wheelRightBack]
#define arm motor[armLeft]=motor[armLeftY]=motor[armRight]=motor[armRightY]
#define claw SensorValue[Pclaw]
#define clrLCD clearLCDLine(0);clearLCDLine(1)
#define AIR_OPEN 1
#define AIR_CLOSE 0
#define kMidPriority 152
string LCDmode="left"; //what mode the LCD is in for autonomous
int time; //used for in autonomous for not letting while loops run forever
bool competitionSwitchIsPluggedIn=false;
int startTime=nSysTime; //time the match starts
void forward(int power=127){wheels=power;}
void backward(int power=127){wheels=-power;}
void left(int power=127)
{
wheelLeft=-power;
wheelRight=power;
}
void right(int power=127)
{
wheelLeft=power;
wheelRight=-power;
}
void stopDrive(){wheels=0;}
void armUp(int power=127){arm=power;}
void armDown(int power=127){arm=-power;}
void stopArm(){arm=0;}
void openClaw(){claw=1;}
void closeClaw(){claw=0;}
void arc(short leftDrive,short rightDrive)
{
wheelLeft=leftDrive;
wheelRight=rightDrive;
}
void wait(int originalWaitTime)
{
wait1Msec(originalWaitTime*recordingBattery/nAvgBatteryLevel);
}
int PID(int current,int target,float Kp)
{
return round(Kp*(target-current));
}
void deploy()
{
openClaw();
wait(100);
armUp();
while(encoderArm>FOLD_OUT_ARM_UP)
wait(1);
armDown();
wait(500);
writeDebugStreamLine("665");
while(encoderArm<FOLD_OUT_ARM_DOWN)
wait(1);
wait(200);
armUp(10);
wait(500);
stopArm();
encoderArm=0;
}
void pre_auton()
{
encoderArm=0;
competitionSwitchIsPluggedIn=bIfiRobotDisabled;
}
task autonomous()
{
clearDebugStream();
startTime=nSysTime;
if(LCDmode=="left")
{
SensorValue[PencoderArm]=0;
resetEncoders;
writeDebugStreamLine("343");
backward();
resetEncoders;
wait1Msec(500);
openClaw();
while(freeWheelLeft>-1050)
{
arm=PID(encoderArm,LOAD_HEIGHT,KpStandard*5);
wait1Msec(10);
}
right();
resetEncoders;
while(freeWheelRight>-95) //go backwards until you are out of the square
{
wait1Msec(5);
writeDebugStreamLine("power: %d",encoderArm);
arm=PID(encoderArm,-10,KpStandard);
}
stopDrive();
int time=0;
while(time++<1000)
{
arm=PID(encoderArm,-10,KpStandard);
wait1Msec(1);
}
forward();
resetEncoders;
time=0;
while(time++<1000)
{
arm=PID(encoderArm,-10,KpStandard);
wait1Msec(1);
}
while(abs(freeWheelLeft)>0 && abs(freeWheelRight)>0)
//go forwards until you hit the wall
{
resetEncoders;
arm=PID(encoderArm,-10,KpStandard);
wait(70);
}
SensorValue[PencoderArm]=0;
closeClaw(); //grab star
backward(15);
armUp(15);
wait1Msec(1000);
arc(-127,-50);
wait(100);
while(abs(freeWheelLeft)>0 && abs(freeWheelLeft)>0) //until it stops at the fence
{
armUp(PID(encoderArm,LOAD_HEIGHT,KpStandard)); //keep the arm above the ground
resetEncoders;
wait(100);
}
armUp(); //score both of the stars
while(encoderArm>SCORE_HEIGHT)
{
wait(1);
writeDebugStreamLine("throw: %d",SensorValue[PencoderArm]);
}
openClaw();
armDown(10);
wait(1000);
}
if(LCDmode=="high")
{
SensorValue[PencoderArm]=0;
resetEncoders;
deploy();
arc(35,127); //arc towards the cube
armDown(5); //push the fork down a bit
while(freeWheelRight<1100)
wait(1);
armUp(60); //lift the cube
closeClaw(); //close the claw
left(100); //turn left sfer so it doesn't throw the cube out of the claw
resetEncoders;
while(freeWheelRight<520)
wait(1);
backward(); //go backwards into the fence
resetEncoders;
time=0;
while(freeWheelLeft>-1000 && time++<1500)
wait(1);
armUp(); //lift arm at full power once youre close enough to the fence
arc(-70,-127); //arc to try to miss the middle cube
while(freeWheelLeft>-1200 && time++<1500)
{
if(encoderArm>SCORE_HEIGHT) //don't slam the arm into the fence
stopArm();
wait(1);
}
stopDrive(); //don't just keep slamming into the fence
if(encoderArm<SCORE_HEIGHT)
wait(1);
stopDrive();
openClaw(); //release the cube
armDown(10);
wait(2000); //wait to stop enemy cubes
armDown();
wait(300);
forward(100); //go forwards towards the stars, giving yourself time to lower the arm
resetEncoders;
wait(1000); //let the arm hit the ground
armDown(5); //put the forklift closer to the ground
time=0;
while(freeWheelLeft<840 && time++<1500)
wait(50);
arc(-120,-127); //arc towards a different part of the fence
resetEncoders;
closeClaw();
while(freeWheelLeft>-550 && time++<2000)
{
armUp(PID(encoderArm,LOAD_HEIGHT,KpStandard)); //hold the arm above the ground
wait(50);
}
backward(50); //back into the fence
armUp(); //lift the stars
while(encoderArm<THROW_HEIGHT)
wait(1);
openClaw(); //throw the stars
while(encoderArm<SCORE_HEIGHT)
wait(1);
armDown(10); //stop with the arm stopping enemy robots
openClaw();
}
if(LCDmode=="right")
{
SensorValue[PencoderArm]=0;
resetEncoders;
backward();
resetEncoders;
wait1Msec(500);
openClaw();
while(freeWheelLeft>-1050)
{
arm=PID(encoderArm,LOAD_HEIGHT,KpStandard*5);
wait1Msec(10);
} //deploy
writeDebugStreamLine("345");
left();
resetEncoders;
while(freeWheelLeft>-102) //go backwards until you are out of the square
{
wait1Msec(5);
arm=PID(encoderArm,-10,KpStandard);
}
stopDrive();
int time=0;
while(time++<1000)
{
arm=PID(encoderArm,-10,KpStandard);
wait1Msec(1);
}
forward();
resetEncoders;
time=0;
while(time++<1000)
{
arm=PID(encoderArm,-10,KpStandard);
wait1Msec(1);
}
while(abs(freeWheelLeft)>0 && abs(freeWheelRight)>0)
//go forwards until you hit the wall
{
resetEncoders;
arm=PID(encoderArm,-10,KpStandard);
wait(70);
}
SensorValue[PencoderArm]=0;
closeClaw(); //grab star
backward(15);
armUp(15);
wait1Msec(1000);
arc(-50,-127); //arc towards the fence
wait(100);
while(abs(freeWheelLeft)>0 && abs(freeWheelLeft)>0) //until it stops at the fence
{
armUp(PID(encoderArm,LOAD_HEIGHT,KpStandard)); //keep the arm above the ground
resetEncoders;
wait(100);
}
armUp(); //score both of the stars
while(encoderArm>SCORE_HEIGHT)
{
wait(1);
}
openClaw();
armDown(10);
wait(1000);
}
writeDebugStreamLine("time: %d",nSysTime-startTime); //display time
}
task usercontrol()
{
startTime=nSysTime;
startTask(LCD);
setTaskPriority(main,kLowPriority);
setTaskPriority(usercontrol,kHighPriority);
setTaskPriority(LCD,kMidPriority);
setTaskPriority(LCDdebug,kMidPriority);
while(true)
{
if(abs(vexRT[Ch2Xmtr2])>CHANNEL_DEAD_BAND || abs(vexRT[Ch3Xmtr2])>CHANNEL_DEAD_BAND)
{
wheelLeft=vexRT[Ch3Xmtr2];
wheelRight=vexRT[Ch2Xmtr2];
}
else
wheels=0;
if(vexRT[Btn6U]==1)
claw=0;
else if(vexRT[Btn6D]==1)
claw=1;
if(vexRT[Btn7U]==1)
encoderArm=0;
if(encoderArm<LOAD_HEIGHT)
arm=vexRT[Btn5U]*127-vexRT[Btn5D]*80+vexRT[Btn7D]*PID(encoderArm,LOAD_HEIGHT,KpStandard);
else
arm=vexRT[Btn5U]*127-vexRT[Btn5D]*20+vexRT[Btn7D]*PID(encoderArm,LOAD_HEIGHT,KpStandard);
wait(USERCONTROL_WAIT_TIME);
}
}
