//blue autonomous for skyrise function
void blueSkyriseautonomous()
{ //-1458.51243217 degrees will position the robot exactly 1.5 squares -- diagonal-- from the skyrise autoloader
//the diameter of the wheen is 4pi inches. The squares are apporx 23.41 inches^2 in area.
clearencoders();
pidDrivecontrol(-500,-510,-490,1,1,2);//brings the drive back to -500 degrees
pidLiftcontrol(-185,-195,-175,-1,-1,5);//should lift the lift to the autoloaders height----- must perfect the height value via trial and error calculations
pidDrivecontrol(350,340,360,1,1,2);//drives forward to grab the skyrise
waitInMilliseconds(150);//waits .15 seconds before executing
SensorValue[CLAW] = 1;//closes claw on skyrise
pidLiftcontrol(-125,-135,-115,-1,-1,3);//should lift the skyrise out of the autoloader
waitInMilliseconds(150);//waits .15 seconds before executing
pidDrivecontrol(-270,-275,-265,1,1,2);//brings the drive back to -270 degrees
pidDrivecontrol(155,150,160,-1,1,3);//turns right to position the skyrise to be scored
pidDrivecontrol(70,65,75,1,1,3);// drives slightly forward to align itself with the skyrise
pidLiftcontrol(180,170,190,-1,-1,4);//lowers lift to score skyrise
SensorValue[CLAW] = 0;//releases claw
pidDrivecontrol(-500,-510,-490,1,1,2);//brings the drive back to -500 degrees to get it out of the way
/***********************************************************************************************************
Deleted code that scores a second skyrise
/**********************************************************************************************************
pidDrivecontrol(-150,-160,-140,1,1,2);//brings the drive back to -720 degrees or moves the robot
pidLiftcontrol(-60,-70,-50,-1,-1,2);//raises lift
pidDrivecontrol(-130,-140,120,1,-1,3);//turns left
pidDrivecontrol(-260,-265,-255,-1,-1,2);//brings the drive back to -360 degrees or moves the robot back one turn
SensorValue[CLAW] = 1;//releases claw
pidLiftcontrol(-125,-135,-115,-1,-1,3);//should lift the lift to the autoloaders height
pidDrivecontrol(155,150,160,-1,1,3);//turns right
pidDrivecontrol(70,65,75,1,1,3);
pidLiftcontrol(180,170,190,-1,-1,4);//lowers lift to score skyrise
SensorValue[CLAW] = 0;//releases claw
*/
}
task main(){
int Forever = 1;
while (Forever == 1){
writeDebugStreamLine("X position: %d", SensorValue[xAccel]);
waitInMilliseconds(250);
writeDebugStreamLine("Y position: %d", SensorValue[yAccel]);
waitInMilliseconds(250);
}
while(Forever == 1) {
writeDebugStreamLine("Angle: %d", SensorValue[Degree]);
waitInMilliseconds(1000);
}
}
task activateLib()
{
for (int i = 0; i < 4; i++) {
motorCtrl[i].active = false;
motorCtrl[i].timer = (i == 0 ? T1 : i == 1 ? T2 : i == 2 ? T3 : T4);
motorCtrl[i].ms = 0;
motorCtrl[i].powerPct = 0;
motorCtrl[i].encoderInUse = false;
motorCtrl[i].brake = false;
}
for (int i = 0; i < PSUEDO_TIMER_SIZE; i++) {
psuedoTimer[i].timerId = 0;
psuedoTimer[i].duration = 0;
psuedoTimer[i].expirationTime = 0;
psuedoTimer[i].recurring = false;
}
startTask(checkPsuedoTimer);
while (true) {
checkMotors();
waitInMilliseconds(5);
}
}
void checkMotorPosition(byte i)
{
if (!motorCtrl[i].encoderInUse) return;
// Get difference between current position and encoder stopping position
int posDiff = abs(SensorValue[motorCtrl[i].encoderPort] - motorCtrl[i].encoderStopPos);
int lastPosDiff = abs(motorCtrl[i].lastKnownPos - motorCtrl[i].encoderStopPos);
// Set new slower motor speed when getting close to final position
if (posDiff < abs(motorCtrl[i].encoderStopPos - motorCtrl[i].encoderSlowPos)) {
turnMotor(motorCtrl[i].motorPort1, motorCtrl[i].motorPort2, motorCtrl[i].slowPct);
}
// Stop motor and timer when encoder position is reached or overshot (or turning the wrong way)
if (posDiff < 50 && posDiff - 1 > lastPosDiff) { // subtract a bit to allow for some sensor error
//writeDebugStreamLine("posDiff: %d, lastPosDiff: %d", posDiff, lastPosDiff);
// Slight motor reverse acts as brake
if (motorCtrl[i].brake) {
turnMotor(motorCtrl[i].motorPort1, motorCtrl[i].motorPort2, (motorCtrl[i].powerPct < 0 ? 20 : -20));
waitInMilliseconds(40);
}
// Experiment with fixing overshot by keeping motor reverse running a little longer
if (abs(SensorValue[motorCtrl[i].encoderPort] - motorCtrl[i].encoderStopPos) > 40) {
//writeDebugStreamLine("Overshot adj, reqt pos: %d, cur pos: %d",
// motorCtrl[i].encoderStopPos, SensorValue[motorCtrl[i].encoderPort]);
waitInMilliseconds(30);
}
// Stop Motor
stopMotor(motorCtrl[i].motorPort1);
if (motorCtrl[i].motorPort1 != motorCtrl[i].motorPort2) stopMotor(motorCtrl[i].motorPort2);
//writeDebugStreamLine("Encoder stop, reqt pos: %d, cur pos: %d",
// motorCtrl[i].encoderStopPos, SensorValue[motorCtrl[i].encoderPort]);
// Clear array
motorCtrl[i].ms = 0;
motorCtrl[i].powerPct = 0;
motorCtrl[i].slowPct = 0;
motorCtrl[i].brake = false;
motorCtrl[i].encoderInUse = false;
motorCtrl[i].active = false;
}
motorCtrl[i].lastKnownPos = SensorValue[motorCtrl[i].encoderPort];
}
task main()
{
untilButtonPress(vexRT[Btn8U]);
setDriveLeftRight(90, 100);
waitInMilliseconds(3000);
stopDriveMotors();
waitInMilliseconds(100);
setDriveLeftRight(100, -100);
waitInMilliseconds(600);
stopDriveMotors();
waitInMilliseconds(100);
setDriveLeftRight(90, 100);
waitInMilliseconds(1000);
stopDriveMotors();
//while (true)
{
//setDriveLeftRight(vexRT[Ch3], vexRT[Ch2]);
//controlFlipper();
//controlScissorLift();
//motor[LeftDriveA] = vexRT[Ch3];
//motor[LeftDriveB] = vexRT[Ch3];
//motor[RightDriveA] = vexRT[Ch2];
//motor[RightDriveB] = vexRT[Ch2];
}
}
task checkPsuedoTimer()
{
while (true) {
for (int i = 0; i < PSUEDO_TIMER_SIZE; i++) {
if (psuedoTimer[i].expirationTime > 0) {
if (psuedoTimer[i].expirationTime <= nSysTime) {
int timerId = psuedoTimer[i].timerId;
if (psuedoTimer[i].recurring) {
psuedoTimer[i].expirationTime = nSysTime + psuedoTimer[i].duration;
} else {
psuedoTimer[i].timerId = 0;
psuedoTimer[i].expirationTime = 0;
psuedoTimer[i].duration = 0;
}
timerCallback(timerId);
}
}
}
waitInMilliseconds(1);
}
}
