///////////////////////////////////////TURN BASED ON GYRO DEGREES EXAMPLE // TurnDegree(-90, 127);//THIS TURNS LEFT 90 DEGREES AT 127 POWER
void TurnDegree(int degrees, int speed, int Timeout)
{
int left = 0, right = 0;
SensorValue[Gyro] = 0;
if (degrees < 0){left = -1; right = 1;}
else if (degrees > 0){left = 1; right = -1; }
float ticks = abs(degrees*6.7);
clearTimer(T3);
while(abs(SensorValue[Gyro]) < ticks)
{
////////////////////////////////////FAILASFE TIMEOUT
if(time1[T3] > TimeOut && TimeOut > 0){ FailSafeEngaged = 1; break;}
////////////////////////////////////////////////////
motor[Left1] = speed*left;
motor[Left2] = speed*left;
motor[Left3] = speed*left;
motor[Right1] = speed*right;
motor[Right2] = speed*right;
motor[Right3] = speed*right;
}
motor[Left1] = (speed*left*-1)/9;
motor[Left2] = (speed*left*-1)/9;
motor[Left3] = (speed*left*-1)/9;
motor[Right1] = (speed*right*-1)/9;
motor[Right2] = (speed*right*-1)/9;
motor[Right3] = (speed*right*-1)/9;
wait1Msec(250);
StopDrive();
}
// This is called when we abort a move because we have hit an endstop.
// It adjusts the end points of the current move to account for how far through the move we got.
void DDA::MoveAborted()
{
for (size_t drive = 0; drive < DRIVES; ++drive)
{
StopDrive(drive);
}
state = completed;
}
void Break2(int time, int speed)
{
motor[Left1] = speed;
motor[Left2] = speed;
motor[Left3] = speed;
motor[Right1] = -speed;
motor[Right2] = -speed;
motor[Right3] = -speed;
wait1Msec(time);
StopDrive();
}
///////////////////////////////////////MOVE FORWARD/BACKWARDS BASED ON TIME EXAMPLE// MoveTime(1000, -127);//THIS MOVES BACKWARDS FOR 1 SECOND
void MoveTime(int time, int speed)
{
motor[Left1] = speed;
motor[Left2] = speed;
motor[Left3] = speed;
motor[Right1] = speed;
motor[Right2] = speed;
motor[Right3] = speed;
wait1Msec(time);
motor[Left1] = (speed*-1)/15;
motor[Left2] = (speed*-1)/15;
motor[Left3] = (speed*-1)/15;
motor[Right1] = (speed*-1)/15;
motor[Right2] = (speed*-1)/15;
motor[Right3] = (speed*-1)/15;
wait1Msec(250);
StopDrive();
}
///////////////////////////////////////MOVE FORWARD/BACKWARDS BASED ON ENCODER READING// while(MoveFwdDist(-10, 127) == 1){DO SOMETHING LIKE INTAKE ON}//THIS MOVES BACKWARDS FOR 10 INCHES AT POWER 127
int MoveDist(int inches, int speed, int TimeOut)
{
int status, direction = 0;
if (MoveCounter == 0 )
{
SensorValue[ENCR] = 0;
MoveCounter = 1;
clearTimer(T3);
}
////////////////////////////////////FAILASFE TIMEOUT
if(time1[T3] > TimeOut && TimeOut > 0) FailSafeEngaged = 1;
////////////////////////////////////////////////////
if (inches < 0){direction = -1;}
else if (inches > 0){direction = 1;}
int ticks = abs(inches*19); ///NEEDS TO BE ADJUSTED BASED ON ENCONDER RATIO
if(abs(SensorValue[ENCR]) < ticks) //ENCODER TO USE FOR DISTANCE
{
motor[Left1] = speed*direction;
motor[Left2] = speed*direction;
motor[Left3] = speed*direction;
motor[Right1] = speed*direction;
motor[Right2] = speed*direction;
motor[Right3] = speed*direction;
status = 1;
}
else
{
motor[Left1] = (speed*direction*-1)/20;
motor[Left2] = (speed*direction*-1)/20;
motor[Left3] = (speed*direction*-1)/20;
motor[Right1] = (speed*direction*-1)/19;
motor[Right2] = (speed*direction*-1)/19;
motor[Right3] = (speed*direction*-1)/19;
delay(330);
StopDrive();
status = 0;
MoveCounter = 0;
}
return status;
}
/*----------------------------------------------------------------------------*/
void BlueAuto2(void)
{
///4 PRELOADS 3 PILE
/// STARTING TYLE IS FURTHERST TYLE
int YouAreGoodToCount = 0;
Transmission(1);
BallCounter = 4;
ShootingMode = 1;
IntakeShoot(6000);//SHOOT 4 PRELOADS // * FAILSAFE POINT
ShootingMode = 4;
DistanceCalculatorTurn(1);
while(DistanceCalculatorTurn(0) < 15) //*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE)
{
Move(-1, 50);
}
Break2(100,-10);
delay(200);
stage = 1;
SetRPM(2200);
DistanceCalculator(1);
while(DistanceCalculator(0) < 12) //*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE)
{
Move(2, 127);
}
clearTimer(T3);
while(SensorValue[IR1] == 1 ) //*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE)
{
IntakeOnAuto();
Move(2, 20);
if(time1[T3] > 6000)
{
FailSafeEngaged = 1;
break;
}
}
if(FailSafeEngaged == 1)Blue1Fail1();/////////////////////RED1FAIL1
IntakePower(0);
Break(100,10);
while(MoveDist(-1, 20,0) == 1) // DO WE NEED FAILSAFE HERE?
{
IntakeOnAuto();
}
while(MoveDist(35, 40,3000) == 1)//17 // DO WE NEED FAILSAFE HERE?
{
IntakeOnAuto();
if(FailSafeEngaged == 1)Blue1FailInfinite();/////////////////////RED1FAIL1
}
IntakePower(0);
while(LineStatus() == 0)
{
Move(-2, 100);
}
Break(100,-10);
while(MoveDist(3, 30,0) == 1);
delay(200);
SetRPM(2200);
while(LineStatus() == 0) //TURN UNTIL YOU SEE THE LINE
{
Move(1, 20);
}
DistanceCalculator(1);
while(LineStatus() == 1) //*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE)
{
FollowLine(40);
if(LineStatusOffset() == 1)
{
DistanceCalculator(1);
YouAreGoodToCount = 1;
}
if(DistanceCalculator(0) > 18 && YouAreGoodToCount == 1)
{
DistanceCalculator(1);
break;
}
}
if(DistanceCalculator(0) < 50 && YouAreGoodToCount == 0)Blue1FailInfinite();//**FAIL GOOD
Break(100,20);
delay(500);
IntakePower(127);///SHOT FIRST PILE
delay(3000);
ShootingMode = 3;
StopDrive();
stage = 1;
DistanceCalculator(1);
while(SensorValue[IR1] == 1 && LineStatus() == 1) //*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE)
{
FollowLine(35);
IntakeOnAuto();
}
Break(100,20);
StopDrive();
clearTimer(T3);
while(stage == 1) // WHAT IF TWO BALLS ARE STUCK FOREVER? TIMER?
{
if(time1[T3] > 10000)
{
break;
}
IntakeOnAuto();
}
clearTimer(T3);
while(LineStatus() == 1 && DistanceCalculator(0) < 37 )//*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE)
{
FollowLine(35);
IntakeOnAuto();
if(time1[T3] > 10000)
{
break;
}
}
MoveTime(500,127);
StopDrive();
delay(500);
IntakePower(127);///SHOT BAR SHOTS
delay(3500);
IntakePower(0);
SetRPM(2200);
stage = 1;
//////////////////////////////////////////////////////SHOT 8 COMPLETE
DistanceCalculator(1);
while(DistanceCalculator(0) < 50) //*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE)
{
Move(-2, 127);
}
Break(200,-15);
delay(200);
DistanceCalculatorTurn(1);
clearTimer(T3);
while(DistanceCalculatorTurn(0) < 62) //*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE)
{
Move(-1, 100);
if(time1[T3] > 5000)
{
FailSafeEngaged = 1;
break;
}
}
if(FailSafeEngaged == 1)Blue1FailInfinite();
Break2(100,10);
delay(300);
DistanceCalculator(1);
clearTimer(T3);
while(DistanceCalculator(0) < 24) //*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE)
{
Move(2, 127);
if(time1[T3] > 5000)
{
FailSafeEngaged = 1;
break;
}
}
if(FailSafeEngaged == 1)Blue1FailInfinite();
clearTimer(T3);
while(SensorValue[IR1] == 1 )//PILE BY THE WALL
{
IntakeOnAuto();
Move(2, 20);
if(time1[T3] > 5000)
{
FailSafeEngaged = 1;
break;
}
}
if(FailSafeEngaged == 1)Blue1FailInfinite();
IntakePower(0);
Break(100,10);
while(MoveDist(-1, 20,0) == 1) // DO WE NEED FAILSAFE HERE?
{
IntakeOnAuto();
}
while(MoveDist(19, 35,2000) == 1) // DO WE NEED FAILSAFE HERE?
{
IntakeOnAuto();
}
IntakePower(0);
///////////////////////////////////////////////////////////////////////////////////PICK UP 3RD
SetRPM(2200);
clearTimer(T3);
while(LineStatus() == 0 )
{
Move(-2, 127);
if(time1[T3] > 5000)
{
FailSafeEngaged = 1;
break;
}
}
if(FailSafeEngaged == 1)Blue1FailInfinite(); //**FAIL GOOD
Break(200,-15);
while(MoveDist(3, 40,0) == 1);
clearTimer(T3);
while(LineStatus() == 0) // TURN UNTILL IT SEES LINE (WHAT IF WE NEVER SEE LINE)
{
Move(1,22);
if(time1[T3] > 5000)
{
FailSafeEngaged = 1;
break;
}
}
if(FailSafeEngaged == 1)Blue1FailInfinite(); //**FAIL GOOD
DistanceCalculator(1);
clearTimer(T3);
while(LineStatus() == 1 && DistanceCalculator(0) < 12)//*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE)
{
FollowLine(35);
if(time1[T3] > 5000)
{
FailSafeEngaged = 1;
break;
}
}
if(FailSafeEngaged == 1)Blue1FailInfinite(); //**FAIL GOOD
Break(100,20);
StopDrive();
delay(500);
IntakePower(127);///SHOT LAST BALLS BAR SHOTS
delay(3000);
TurnDegree(-45,100,2000);
while(MoveDist(24, 40,0) == 1);
ShootingMode = 4;
KillALL();
while(1);
}
///////////////////////////////////////TURN EVERYTHING OFF
void KillALL(void)
{
StopDrive();
IntakePower(0);
ShootingMode = 4;
}
void RedAuto1(void)
{
int Launch = 0;
Transmission(1);
BallCounter = 4;
ShootingMode = 1;
IntakeShoot(6000);//SHOOT 4 PRELOADS // * FAILSAFE POINT
ShootingMode = 4;
stage = 1;
TurnDegree(-10,40,0); // DO WE NEED FAILSAFE HERE?
delay(200);
Move(2,127);
delay(500);
clearTimer(T3);
while(SensorValue[IR1] == 1) //*FAILSAFE ?WHAT IF WE DONT SEE ANY BALLS
{
IntakeOnAuto();
Move(2,20);
if(time1[T3] > 8000)
{
FailSafeEngaged = 1;
break;
}
}
if(FailSafeEngaged == 1)Red1Fail1();/////////////////////RED1FAIL1
IntakePower(0);
Break(100,10);
while(MoveDist(-1, 20,0) == 1) // DO WE NEED FAILSAFE HERE?
{
IntakeOnAuto();
}
while(MoveDist(40, 50,0) == 1) //*FAILSAFE ?WHAT IF WE ARE BLOCKED (ROBOT FINISHED ABOUT MID COURT)
{
if(FailSafeEngaged == 1)Red1Fail1();/////////////////////RED1FAIL1
IntakeOnAuto();
}
IntakePower(0);
TurnDegree(-45,127,2000); //*FAILSAFE ?WHAT IF WE ARE BLOCKED
if(FailSafeEngaged == 1)Red1FailInfinite();
delay(200);
SetRPM(2250);//LAUNCHER ON (NEXT SHOOTING RPM)
clearTimer(T3);
while(LineStatus() == 0) //*FAILSAFE AT WHAT POINT DO WE STOP MOVING? WHAT IF WE ARE OF COURSE?
{
Move(-2,30);
if(time1[T3] > 3500)
{
FailSafeEngaged = 1;
break;
}
}
if(FailSafeEngaged == 1)Red1FailInfinite();
Break(100,-20);
delay(200);
while(MoveDist(7, 30,0) == 1){} // DO WE NEED FAILSAFE HERE?
clearTimer(T3);
while(LineStatus() == 0) // TURN UNTILL IT SEES LINE (WHAT IF WE NEVER SEE LINE)
{
Move(1,22);
if(time1[T3] > 3000)
{
FailSafeEngaged = 1;
break;
}
}
if(FailSafeEngaged == 1)Red1FailInfinite();
while(LineStatus() == 1 && LineStatusOffset() == 0) //*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE)
{
FollowLine(40);
if(LineStatus() == 1 && LineStatusOffset() == 1) Launch = 1;//WE REACHED THE CROSS(MIDDLE OF FIELD SET FLAG
}
if(LineStatus() == 1 && LineStatusOffset() == 1) Launch = 1;
Break(100,20);
StopDrive();
delay(300);
if(Launch) //IF LAUNCH FLAG WAS SATISFIED EARLIER THEN WE CAN LAUNCH BALLS
{
IntakePower(127); // SHOOT MID COURT
delay(3000);
}
stage = 1;
ShootingMode = 3;
DistanceCalculator(1);
while(SensorValue[IR1] == 1 && LineStatus() == 1 && DistanceCalculator(0) < 55) //*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE)
{
FollowLine(35);
IntakeOnAuto();
}
Break(100,20);
StopDrive();
while(stage == 1) // WHAT IF TWO BALLS ARE STUCK FOREVER? TIMER?
{
IntakeOnAuto();
}
while(LineStatus() == 1 && DistanceCalculator(0) < 55)//*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE)
{
FollowLine(35);
IntakeOnAuto();
}
MoveTime(300,127);
StopDrive();
IntakePower(127);///SHOT LAST BALLS BAR SHOTS
delay(4000);
stage = 1;
///////////
KillALL();
while(1);
}
// This is called by the interrupt service routine to execute steps.
// It returns true if it needs to be called again on the DDA of the new current move, otherwise false.
// This must be as fast as possible, because it determines the maximum movement speed.
bool DDA::Step()
{
bool repeat;
uint32_t numReps = 0;
do
{
// Keep this loop as fast as possible, in the case that there are no endstops to check!
// Check endstop switches and Z probe if asked
if (endStopsToCheck != 0) // if any homing switches or the Z probe is enabled in this move
{
if ((endStopsToCheck & ZProbeActive) != 0) // if the Z probe is enabled in this move
{
// Check whether the Z probe has been triggered. On a delta at least, this must be done separately from endstop checks,
// because we have both a high endstop and a Z probe, and the Z motor is not the same thing as the Z axis.
switch (reprap.GetPlatform()->GetZProbeResult())
{
case EndStopHit::lowHit:
MoveAborted(); // set the state to completed and recalculate the endpoints
reprap.GetMove()->ZProbeTriggered(this);
break;
case EndStopHit::lowNear:
ReduceHomingSpeed();
break;
default:
break;
}
}
for (size_t drive = 0; drive < AXES; ++drive)
{
if ((endStopsToCheck & (1 << drive)) != 0)
{
switch(reprap.GetPlatform()->Stopped(drive))
{
case EndStopHit::lowHit:
endStopsToCheck &= ~(1 << drive); // clear this check so that we can check for more
if (endStopsToCheck == 0 || reprap.GetMove()->IsCoreXYAxis(drive)) // if no more endstops to check, or this axis uses shared motors
{
MoveAborted();
}
else
{
StopDrive(drive);
}
reprap.GetMove()->HitLowStop(drive, this);
break;
case EndStopHit::highHit:
endStopsToCheck &= ~(1 << drive); // clear this check so that we can check for more
if (endStopsToCheck == 0 || reprap.GetMove()->IsCoreXYAxis(drive)) // if no more endstops to check, or this axis uses shared motors
{
MoveAborted();
}
else
{
StopDrive(drive);
}
reprap.GetMove()->HitHighStop(drive, this);
break;
case EndStopHit::lowNear:
// Only reduce homing speed if there are no more axes to be homed.
// This allows us to home X and Y simultaneously.
if (endStopsToCheck == (1 << drive))
{
ReduceHomingSpeed();
}
break;
default:
break;
}
}
}
if (state == completed) // we may have completed the move due to triggering an endstop switch or Z probe
{
break;
}
}
// Generate any steps that are now due, overdue, or will be due very shortly
DriveMovement* dm = firstDM;
if (dm == nullptr) // I don't think this should happen, but best to be sure
{
state = completed;
break;
}
const uint32_t elapsedTime = (Platform::GetInterruptClocks() - moveStartTime) + minInterruptInterval;
while (elapsedTime >= dm->nextStepTime) // if the next step is due
{
size_t drive = dm->drive;
++numReps;
reprap.GetPlatform()->StepHigh(drive);
firstDM = dm->nextDM;
bool moreSteps = (isDeltaMovement && drive < AXES)
? dm->CalcNextStepTimeDelta(*this, drive, true)
: dm->CalcNextStepTimeCartesian(*this, drive, true);
if (moreSteps)
{
InsertDM(dm);
}
else if (firstDM == nullptr)
{
state = completed;
reprap.GetPlatform()->StepLow(drive);
goto quit; // yukky multi-level break, but saves us another test in this time-critical code
}
reprap.GetPlatform()->StepLow(drive);
dm = firstDM;
//uint32_t t3 = Platform::GetInterruptClocks() - t2;
//if (t3 > maxCalcTime) maxCalcTime = t3;
//if (t3 < minCalcTime) minCalcTime = t3;
}
repeat = reprap.GetPlatform()->ScheduleInterrupt(firstDM->nextStepTime + moveStartTime);
} while (repeat);
quit:
if (numReps > maxReps)
{
maxReps = numReps;
}
if (state == completed)
{
uint32_t finishTime = moveStartTime + clocksNeeded; // calculate how long this move should take
Move *move = reprap.GetMove();
move->CurrentMoveCompleted(); // tell Move that the current move is complete
return move->StartNextMove(finishTime); // schedule the next move
}
return false;
}
