int moveStraight(int distance, int inchesOrEncoders = true, int speed = defaultMovementSpeed)
{
resetEncoders();
int encoderDistance = 0;
if(inchesOrEncoders){
encoderDistance = inchesToEncoder(distance);
}
else {
encoderDistance = distance;
}
if (distance > 0){
SetMotors(speed,speed);
while (averageEncoders() < encoderDistance){
wait1Msec(3);
}
}
else{
SetMotors(-speed,-speed);
while (averageEncoders() > encoderDistance){
wait1Msec(3);
}
}
SetMotors(0,0);
return averageEncoders();
}
void DriveTrain::Turn(float angle){
gyro->Reset();
int breakout = 0;
double p, i, d, err, lastErr;
p = i = d = err = 0;
lastErr = gyro->GetAngle()-angle;
while(1) {
err = gyro->GetAngle()-angle;
p = err;
i = i + err * TURN_T;
d = (err-lastErr)/TURN_T;
lastErr = err;
if(fabs(err) > 20)
i = 0;
double leftmotor= p*TURN_P + d*TURN_D + i*TURN_I;
if(fabs(angle - gyro->GetAngle()) > 1.5){
SetMotors(leftmotor, -leftmotor);
breakout = 0;
} else {
breakout++;
if (breakout >= 2){
SetMotors(0, 0);
break;
}
}
Wait(TURN_T);
}
}
void
Arm::GoToReleasePosition() // sets the target position
{
#if 1
armTimer->Reset();
armTimer->Start();
timeToGoToReleaseAngle = 1.00;
#else
// targetPosition = degrees;
startTime = (float)armTimer->Get();
if(angleOfArm > ReleaseAngle)
{
SetMotors(manualUpSpeed);
}
else
{
SetMotors(-manualDownSpeed);
}
timeToGoToReleaseAngle = (fabs(angleOfArm - ReleaseAngle) / 90) * timeToGoNinetyDegrees;
#endif
SetMotors(-manualDownSpeed);
controlMode = ReleasePositionMode;
}
void DriveTrain::GoDistance(float distance){
encoder_center->Reset();
gyro->Reset();
double p, i, d, err, lastErr;
double multiplier = 0.5;
p = i = d = err = 0;
lastErr = gyro->GetAngle();
while(encoder_center->GetDistance() < distance) {
err = gyro->GetAngle();
p = err;
i = i + err * FWD_T;
d = (err-lastErr)/FWD_T;
lastErr = err;
double diff= p*FWD_P + d*FWD_D + i*FWD_I;
if (encoder_center->GetDistance() > distance - 7)
{
multiplier = (distance - encoder_center->GetDistance()) / 14;
if (multiplier < .2)
{
multiplier = 0.2;
}
}
SetMotors((multiplier + diff), (multiplier-diff));
Wait(FWD_T);
}
coast->Set(0);
SetMotors(0,0);
Wait(1);
coast->Set(1);
}
void DriveTrain::GoDistance(float distance){
encoder_right->Reset();
encoder_left->Reset();
while (-0.5*(encoder_left->GetDistance() + encoder_right->GetDistance()) < distance){
SetMotors(0.2,0.2);
}
coast->Set(0);
SetMotors(0,0);
Wait(1);
coast->Set(1);
}
// TankDrive (uses Y axis)
void DriveTrain::TankDrive(Joystick *left, Joystick *right)
{
float l = -1 * left->GetY();
float r = -1 * right->GetY();
SetMotors(l, r);
}
void Arm::GoToUpPosition()
{
if(!motorA->GetReverseLimitOK())
{
SetMotors(maxSpeed);
}
}
void Arm::GoToDownPosition()
{
if(!motorA->GetForwardLimitOK())
{
SetMotors(-maxSpeed);
}
}
void FindLine()
{
int iCur;
int iProc;
int wInit;
DebugStop("Find Line Start");
Lookdown();
iCur = iSight;
wInit = wLD;
iProc = start_process(Move(1000,1000));
while (iSight == iCur && !fBlocked && !fStalled && !fBall)
{
printf("FindLine: %d\n", iSight);
defer();
Lookdown();
}
printf("I:%d, B:%d S:%d\n", iSight, fBlocked, fStalled);
kill_process(iProc);
SetMotors(0,0);
if (fBlocked || fStalled)
Unbind();
}
/* Assume starting well aligned to line (black on left, white on right).
If we leave the iMid region - we return (and set fLoseTrack) */
void LineFollower()
{
int pwrLeft;
int pwrRight;
InitEncoders();
while (!fStalled && !fBlocked)
{
Lookdown();
pwrLeft = WProportion(wLD, wBlack, wWhite, pwrTrackMax, pwrTrackMin);
pwrRight = WProportion(wLD, wBlack, wWhite, pwrTrackMin, pwrTrackMax);
printf("Follow: %d - %d\n", pwrRight, pwrLeft);
SetMotors(pwrLeft, pwrRight);
ReadEncoders();
}
SetMotors(0,0);
}
void DriveTrain::Turn(float angle){
gyro->Reset();
float speed = 0;
while(1) {
speed = fabs(1-(gyro->GetAngle()/angle));
//printf("%f\n", speed);
if((angle - gyro->GetAngle()) < -5){
SetMotors(-0.5*speed, 0.5*speed);
//printf("CW ");
} else if ((angle - gyro->GetAngle()) > 5){
SetMotors(0.5*speed, -0.5*speed);
//printf("CCW ");
} else {
SetMotors(0,0);
break;
}
}
}
void DriveTrain::GoDistance(float distance){
encoder_center->Reset();
gyro->Reset();
double lastAngle = 0;
double left, right;
while (encoder_center->GetDistance() < distance){
double p = FWD_P * gyro->GetAngle();
double d = FWD_D * ((gyro->GetAngle()-lastAngle)/FWD_T);
lastAngle = gyro->GetAngle();
left = (0.5 + p - d);
right = (0.5 - p + d);
SetMotors((1*(left/2)), (1*(right/2)));
Wait(FWD_T);
}
coast->Set(0);
SetMotors(0,0);
Wait(1);
coast->Set(1);
}
void turnDegrees(int deg)
{
resetHeading();
SetMotors(turnSpeed*sign(deg),turnSpeed*sign(deg)*-1);
if (deg > 0){
while (heading < deg){
wait1Msec(3);
}
}
else{
while (heading > deg){
wait1Msec(3);
}
}
SetMotors(0,0);
}
//The Drive
void Tank(int y1, int y2)
{
int powLeft = scaleJoystick(y1)/slowMode(joy1Btn(joySlow),joy1Btn(joySuperSlow))*1.0; // Left hand joystick, y value.
int powRight = scaleJoystick(y2)/slowMode(joy1Btn(joySlow),joy1Btn(joySuperSlow))*1.0; // Right hand joystick, y value.
if (!tankSpeed){
// This defines our speed.
powLeft = powLeft/6;
powRight = powRight/6;
}
SetMotors(powLeft, powRight);
// writeDebugStreamLine("scaleJoystick %d %d", y1, y2);
}
void pouncer(){
// Forward
if(joy2Btn(4)){
SetMotors(50, 50);
wait10Msec(.25*100);
SetMotors(0, 0);
SetCrane(-100, 0);
wait10Msec(.35*100);
SetCrane(0, 0);
SetCrane(100, 0);
wait10Msec(.65*100);
SetCrane(0, 0);
}
// Left
if(joy2Btn(1)){
SetMotors(25, 75);
wait10Msec(.25*100);
SetMotors(0, 0);
SetCrane(-100, 0);
wait10Msec(.35*100);
SetCrane(0, 0);
SetCrane(100, 0);
wait10Msec(.65*100);
SetCrane(0, 0);
}
// Right
if(joy2Btn(3)){
SetMotors(75, 25);
wait10Msec(.25*100);
SetMotors(0, 0);
SetCrane(-100, 0);
wait10Msec(.35*100);
SetCrane(0, 0);
SetCrane(100, 0);
wait10Msec(.65*100);
SetCrane(0, 0);
}
// No move
if(joy2Btn(2)){
SetCrane(-100, 0);
wait10Msec(.35*100);
SetCrane(0, 0);
SetCrane(100, 0);
wait10Msec(.65*100);
}
}
//Calculate new angle and position given starting angle and position when timer started, elapsed time and keystate when last changed.
void MoveCamera(Point3d *cameraPos, Point3d *cameraAngle, Point3d baselinePos, float baselineAngle, unsigned int elapsed, int keys) {
float distance;
if(blocked)
SetMotors(0,0);
switch (keys & (KEY_RIGHT_BACK+KEY_RIGHT_FORWARD+KEY_LEFT_BACK+KEY_LEFT_FORWARD)) {
case KEY_RIGHT_BACK+KEY_LEFT_FORWARD: { //rotate right
if (blocked) {
blocked=false;
CloseMotors();
}
cameraAngle->y=baselineAngle + (PI * 2.0 * elapsed) / rotateSleep;
}
break;
case KEY_RIGHT_FORWARD+KEY_LEFT_BACK: { //rotate left
if (blocked) {
blocked=false;
CloseMotors();
}
cameraAngle->y=baselineAngle - (PI * 2.0 * elapsed) / rotateSleep;
}
break;
case KEY_RIGHT_FORWARD: { //one wheel rotate left
if (blocked) {
blocked=false;
CloseMotors();
}
cameraAngle->y=baselineAngle - (PI * elapsed) / rotateSleep;
}
break;
case KEY_LEFT_FORWARD: { //one wheel rotate right
if (blocked) {
blocked=false;
CloseMotors();
}
cameraAngle->y=baselineAngle + (PI * elapsed) / rotateSleep;
}
break;
case KEY_RIGHT_BACK: { //one wheel rotate left
if (blocked) {
blocked=false;
CloseMotors();
}
cameraAngle->y=baselineAngle + (PI * elapsed) / rotateSleep;
}
break;
case KEY_LEFT_BACK: { //one wheel rotate left
if (blocked) {
blocked=false;
CloseMotors();
}
cameraAngle->y=baselineAngle - (PI * elapsed) / rotateSleep;
}
break;
case KEY_RIGHT_FORWARD+KEY_LEFT_FORWARD: { //forward
//First, check if anything is in our way
int ob = LineHitsObject(*cameraPos, *cameraAngle);
if (ob != 0) {
//something's in the way, calculate the distance to it
Point3d offset = SubtractPoint(world.objects[ob].centre, *cameraPos);
distance = qsqrt(offset.x * offset.x + offset.z * offset.z);
//if we're too close, stop engines and refuse to proceed
if (distance < 30 && !blocked) {
if (world.objects[ob].colour.R==255 && world.objects[ob].colour.G==0 && world.objects[ob].colour.B==0) {
//it's a barrel - red barrels always explode in FPSs!
LoseLife("Beware barrels!");
} else {
OpenMotors();
SetMotors(0,0);
PlaySound(SOUND_HIT);
blocked=true;
}
break;
}
}
if (blocked) {
blocked=false;
CloseMotors();
}
distance=(20.0 * elapsed)/FORWARDSLEEP;
cameraPos->x=baselinePos.x + (distance * qsin(baselineAngle));
cameraPos->z=baselinePos.z + (distance * qcos(baselineAngle));
};
break;
case KEY_RIGHT_BACK+KEY_LEFT_BACK: { //backwards
//First, check if anything is in our way
Point3d dir=*cameraAngle;
dir.y=ClipAngle(dir.y+PI);
int ob = LineHitsObject(*cameraPos, dir);
if (ob != 0) {
//something's in the way, calculate the distance to it
Point3d offset = SubtractPoint(world.objects[ob].centre, *cameraPos);
distance = qsqrt(offset.x * offset.x + offset.z * offset.z);
//if we're too close, stop engines and refuse to proceed
if (distance < 30 && !blocked) {
if (world.objects[ob].colour.R==255 && world.objects[ob].colour.G==0 && world.objects[ob].colour.B==0) {
//it's a barrel - red barrels always explode in FPSs!
LoseLife("Beware barrel!s");
} else {
OpenMotors();
SetMotors(0,0);
PlaySound(SOUND_HIT);
blocked=true;
}
break;
}
}
if (blocked) {
blocked=false;
CloseMotors();
}
distance=(20.0 * elapsed)/FORWARDSLEEP;
cameraPos->x=baselinePos.x - (distance * qsin(baselineAngle));
cameraPos->z=baselinePos.z - (distance * qcos(baselineAngle));
};
}
//normalise ret to 0..2PI
cameraAngle->y = ClipAngle(cameraAngle->y);
}
//-------------------------------------------------------
void SetDirection(int angle,int speed,int calb){
if (calb < 0){
switch(angle){
case 0: SetMotors(-speed,-speed,speed+calb,speed+calb); break;
case 45: SetMotors(calb,-speed,calb,speed+calb); break;
case 90: SetMotors(speed+calb,-speed,-speed,speed+calb); break;
case 135: SetMotors(speed+calb,calb,-speed,calb); break;
case 180: SetMotors(speed+calb,speed+calb,-speed,-speed); break;
case 225: SetMotors(calb,speed+calb,calb,-speed); break;
case 270: SetMotors(-speed,speed+calb,speed+calb,-speed); break;
case 315: SetMotors(-speed,calb,speed+calb,calb); break;
default: SetMotors(0,0,0,0); break;
}
}
else{
switch(angle){
case 0: SetMotors(-speed+calb,-speed+calb,speed,speed); break;
case 45: SetMotors(calb,-speed+calb,calb,speed); break;
case 90: SetMotors(speed,-speed+calb,-speed+calb,speed); break;
case 135: SetMotors(speed,calb,-speed+calb,calb); break;
case 180: SetMotors(speed,speed,-speed+calb,-speed+calb); break;
case 225: SetMotors(calb,speed,calb,-speed+calb); break;
case 270: SetMotors(-speed+calb,speed,speed,-speed+calb); break;
case 315: SetMotors(-speed+calb,calb,speed,calb); break;
default: SetMotors(0,0,0,0); break;
}
}
}
void
Arm::PeriodicService()
{
if (motorA == NULL) return;
CheckJoystick();
CheckManualControl();
switch(controlMode)
{
// printf("Arm ControlMode = %d\n", controlMode);
case HomingMode:
SetMotors(-maxSpeed/2); //both move down at half speed, can change to accomodate for others
if(IsDownLimitSwitchActive())
{
angleOfArm = 90;
motorA->Set(0);
//zeroAngleVolts = magEncoder->GetVoltage();
controlMode = ManualMode;
}
break;
case ReleasePositionMode:
{
if(armTimer->Get() >= timeToGoToReleaseAngle)
{
SetMotors(0);
angleOfArm = ReleaseAngle;
controlMode = ManualMode;
}
break;
}
case ManualMode:
break;
case ToUpLimitSwitchMode:
{
if(IsUpLimitSwitchActive())
{
SetMotors(0);
angleOfArm = 0;
controlMode = ManualMode;
}
break;
}
case ToDownLimitSwitchMode:
{
if(IsDownLimitSwitchActive())
{
SetMotors(0);
angleOfArm = 90;
controlMode = ManualMode;
}
break;
}
default:
break;
}
}
void Winch::handle()
{
// TODO: improve readability
switch(m_State)
{
case READY_TO_FIRE:
{
//printf("LOCKED (READY_TO_FIRE)\n");
m_Lock->Set(false);
m_Fire->Set(false);
SetMotors(0);
break;
}
case PREFIRING:
{
m_Lock->Set(true);
double error = m_Pot->GetAverageVoltage() - m_Setpoint;
double change = error - (m_PreviousVoltage - m_Setpoint);
const double PID_P = CONSTANT("PID_P");
const double PID_D = 3*CONSTANT("PID_D");
double output = PID_P*error + PID_D*change;
if(fabs(error) < CONSTANT("LockBand"))
{
//printf("Pulling freespin\n");
m_Fire->Set(true);
}
if(CowLib::UnitsPerSecond(fabs(change)) > 10)
{
printf("Freespinning detected\n");
SetMotors(0);
m_State = FIRING;
break;
}
if(output < 0)
{
output = CowLib::LimitMix(output, 0.225);
}
//printf("PREFIRING: %f\t%f\n", error, output);
SetMotors(output);
break;
}
case FIRING:
{
//printf("FIRING\n");
m_Lock->Set(true);
m_Fire->Set(true);
SetMotors(0);
if(m_Pot->GetAverageVoltage() > CONSTANT("FiredSetpoint") && TimeSinceLastFireStop() > 2)
{
m_LastFireStopTime = Timer::GetFPGATimestamp();
}
if(TimeSinceLastFireStop() > 0.2 && m_Pot->GetAverageVoltage() > CONSTANT("FiredSetpoint"))
{
m_Setpoint += 0.01;
m_State = RELOADING;
}
break;
}
case RELOADING:
{
//printf("RELOADING\n");
m_Lock->Set(true);
m_Fire->Set(false);
double error = m_Pot->GetAverageVoltage() - m_Setpoint;
double change = error - (m_PreviousVoltage - m_Setpoint);
const double PID_P = CONSTANT("PID_P");
const double PID_D = CONSTANT("PID_D");
double output = PID_P*error + PID_D*change;
if(fabs(error) < CONSTANT("LockBand"))
{
printf("Locking\n");
m_LastReloadStopTime = Timer::GetFPGATimestamp();
m_State = LOCKING;
break;
}
if(output < 0)
{
output = CowLib::LimitMix(output, 0.225);
}
static unsigned int printCount = 0;
if(printCount % 10 == 0)
{
//printf("Winch: %f; Setpoint: %f; Output: %f\n", m_Pot->GetAverageVoltage(), m_Setpoint, output);
printCount = 0;
}
printCount++;
SetMotors(output);
break;
}
case LOCKING:
{
//printf("LOCKING\n");
m_Fire->Set(false);
double error = m_Pot->GetAverageVoltage() - m_Setpoint;
double change = error - (m_PreviousVoltage - m_Setpoint);
const double PID_P = CONSTANT("PID_P");
const double PID_D = CONSTANT("PID_D");
double output = PID_P*error + PID_D*change;
if(TimeSinceLastReloadStop() > 0.125)
{
//printf("Locking\n");
m_Lock->Set(false);
}
else
{
m_Lock->Set(true);
}
if(TimeSinceLastReloadStop() > 0.8)
{
printf("Ending PID\n");
SetMotors(0);
m_State = READY_TO_FIRE;
break;
}
if(output < 0)
{
output = CowLib::LimitMix(output, 0.225);
}
SetMotors(output);
break;
}
case UNLOCKING:
{
//printf("UNLOCKING\n");
m_Fire->Set(false);
m_Lock->Set(true);
SetMotors(0);
//printf("Starting unlock\n");
if(fabs(m_Pot->GetAverageVoltage() - m_LastUnlockVoltage) > 0.005)
{
printf("Unlock complete\n");
m_State = m_NextState;
}
break;
}
case HOLDING:
{
//printf("HOLDING\n");
m_Lock->Set(true);
m_Fire->Set(false);
double error = m_Pot->GetAverageVoltage() - m_HoldVoltage;
double change = error - (m_PreviousVoltage - m_HoldVoltage);
const double PID_P = CONSTANT("PID_P");
const double PID_D = CONSTANT("PID_D");
double output = PID_P*error + PID_D*change;
if(output < 0)
{
output = CowLib::LimitMix(output, 0.225);
}
SetMotors(output);
m_State = RELOADING;
break;
}
case DESPRINGING:
{
//printf("DESPRINGING\n");
m_Lock->Set(true);
m_Fire->Set(false);
SetMotors(0);
break;
}
}
m_PreviousVoltage = m_Pot->GetAverageVoltage();
}
void Arm::CheckManualControl()
{
static State status = Still;
bool isDownPressed = joystick->GetRawButton(ArmManualDown);
bool isUpPressed = joystick->GetRawButton(ArmManualUp);
static bool wasDownPressed = false;
static bool wasUpPressed = false;
if(isDownPressed && !IsDownLimitSwitchActive())
{
#if SIMULATOR
// printf("Arm: Moving Down\n");
#endif
SetMotors(-manualDownSpeed);
controlMode = ManualMode;
if(!wasDownPressed)
{
startTime = (float)armTimer->Get();
status = GoingDown;
}
}
else if(isUpPressed && !IsUpLimitSwitchActive())
{
#if SIMULATOR
// printf("Arm: Up\n");
#endif
SetMotors(manualUpSpeed);
controlMode = ManualMode;
if(!wasUpPressed)
{
startTime = (float)armTimer->Get();
status = GoingUp;
}
}
else
{
if(controlMode == ManualMode)
{
SetMotors(0);
}
float dTime = (float)armTimer->Get() - startTime;
if (status == GoingUp)
{
angleOfArm -= dTime * 90 / timeToGoNinetyDegrees;
}
else if (status == GoingDown)
{
angleOfArm += dTime * 90 / timeToGoNinetyDegrees;
}
}
wasDownPressed = isDownPressed;
wasUpPressed = isUpPressed;
}
//Обрабатываем значения HoldingRegisters
void ModbusSaver()
{
switch (usRegHoldingBuf[MB_OFFSET+MB_COMMAND])
{
case 1:
wdt_enable(WDTO_15MS); // enable watchdog
while(1); // wait for watchdog to reset processor break;
break;
case 2:
ADXL345_Calibrate();
break;
}
usRegHoldingBuf[MB_OFFSET+MB_COMMAND] = 0;
if (usRegHoldingBuf[MB_OFFSET+MB_LED_BLUE])
{
LED_On(LED_BLUE);
}
else
{
LED_Off(LED_BLUE);
}
if (usRegHoldingBuf[MB_OFFSET+MB_LED_GREEN])
{
LED_On(LED_GREEN);
}
else
{
LED_Off(LED_GREEN);
}
if (bit_is_set(usRegHoldingBuf[MB_OFFSET+MB_SOUND], 0))
{
Sound_On();
}
else
{
Sound_Off();
}
if (usRegHoldingBuf[MB_OFFSET+MB_ALL]<16000UL)
{
usRegHoldingBuf[MB_OFFSET+MB_ALL]=16000UL;
}
if (bit_is_set(usRegHoldingBuf[MB_OFFSET+MB_MANUAL], 4))
{
float speeds[4];
speeds[FRONT_LEFT] = (float)usRegHoldingBuf[MB_OFFSET + MB_FRONT_LEFT];
speeds[FRONT_RIGHT] = (float)usRegHoldingBuf[MB_OFFSET + MB_FRONT_RIGHT];
speeds[REAR_LEFT] = (float)usRegHoldingBuf[MB_OFFSET + MB_REAR_LEFT];
speeds[REAR_RIGHT] = (float)usRegHoldingBuf[MB_OFFSET + MB_REAR_RIGHT];
SetMotors(speeds);
}
else
{
usRegHoldingBuf[MB_OFFSET + MB_FRONT_LEFT] = counter[FRONT_LEFT];
usRegHoldingBuf[MB_OFFSET + MB_FRONT_RIGHT] = counter[FRONT_RIGHT];
usRegHoldingBuf[MB_OFFSET + MB_REAR_LEFT] = counter[REAR_LEFT];
usRegHoldingBuf[MB_OFFSET + MB_REAR_RIGHT] = counter[REAR_RIGHT];
}
//t_Ox.value = 0;
//t_Oy.value = 0;
t_Ox.array[0] = usRegHoldingBuf[2];
t_Ox.array[1] = usRegHoldingBuf[3];
t_Oy.array[0] = usRegHoldingBuf[4];
t_Oy.array[1] = usRegHoldingBuf[5];
t_Oz.array[0] = usRegHoldingBuf[6];
t_Oz.array[1] = usRegHoldingBuf[7];
ModbusEEPROMLoader();
}
void Move(int cLMax, int cRMax)
{
int sL;
int sR;
float tL;
float tR;
float tMax;
int sLSet;
int sRSet;
long ms;
long msLimit;
printf("Move: %d, %d\n", cLMax, cRMax);
if (fAssertEnable)
StartPress();
msLimit = mseconds() + 2000L;
InitEncoders();
if (cLMax < 0)
{
cLMax = -cLMax;
sL = -pwrMax;
}
else
sL = pwrMax;
if (cRMax < 0)
{
cRMax = -cRMax;
sR = -pwrMax;
}
else
sR = pwrMax;
while (cL < cLMax || cR < cRMax)
{
if (stop_button())
break;
ReadEncoders();
if (FBallCapture())
break;
if (fStalled || fBlocked)
{
if (!fForce)
{
beep();beep();beep();
printf("Abort Move\n");
break;
}
ms = mseconds();
if (ms >= msLimit)
{
beep();beep();beep();
printf("Time out\n");
break;
}
}
if (cL >= cLMax)
sL = 0;
if (cR >= cRMax)
sR = 0;
/* printf("L: %d, R: %d\n", cL, cR); */
tL = Parametric(cL, cLMax);
tR = Parametric(cR, cRMax);
sRSet = sR;
sLSet = sL;
/* BUG: Can need less than 3/4 speed - also need not retart balanced
motors when both can track the same! */
if (tL > tR)
sLSet = sL*3/4;
else
sRSet = sR*3/4;
SetMotors(sLSet, sRSet);
}
SetMotors(0,0);
}
bool Run()
{
//Main loop - get keypresses and interpret them
keystate=GetRemoteKeys();
if(keystate != oldKeystate) {
oldKeystate = keystate;
if(keystate & KEY_HOME) {
return false;
}
if (mode == 1 || mode == 2) { //menu navigation is only relevant to menu modes
if(keystate & KEY_LEFT_BACK) {
//Menu down
if (menuItem<MAXMENU) {
menuItem++;
if(menuItem==3 || menuItem==4) {
menuNum=6;
} else if (menuItem == 5) {
menuNum=3;
} else {
menuNum=1;
}
}
}
if(keystate & KEY_LEFT_FORWARD) {
//Menu up
if (menuItem>1) {
menuItem--;
if(menuItem==3 || menuItem==4) {
menuNum=6;
} else if (menuItem == 5) {
menuNum=3;
} else {
menuNum=1;
}
}
}
}
if (mode == 1) {
if(keystate & KEY_RIGHT_FORWARD) {
//Menu down
if (menuNum<24) {
menuNum++;
}
}
if(keystate & KEY_RIGHT_BACK) {
//Menu up
if (menuNum>1) {
menuNum--;
}
}
if(keystate & KEY_INPUT1) {
//Clear
PlaySound(SOUND_FIRE);
steps=0;
menuItem=1;
menuNum=1;
mode=1;
OpenMotors();
}
if(keystate & KEY_INPUT2) {
//Menu select
if(steps+1<MAXSTEPS) {
steps++;
moveList[steps].cmd=menuItem;
moveList[steps].num=menuNum;
PlaySound(SOUND_BEEP);
}
}
if(keystate & KEY_MENU) {
//Switch to second menu
mode=2;
menuItem=1;
menuNum=1;
PlaySound(SOUND_BEEP);
}
} else if (mode == 2) {
if(keystate & KEY_INPUT1) {
//Switch back to first menu
mode=1;
menuItem=1;
menuNum=1;
PlaySound(SOUND_BEEP);
}
if (keystate & KEY_INPUT2) {
//Menu select
switch(menuItem) {
case 1: { //Free Roam
mode=4;
PlaySound(SOUND_BEEP);
CloseMotors();
} break;
case 2: { //Load route
LoadRoute();
ClearScreen();
PlaySound(SOUND_BEEP);
SetTextColor(white);
DrawText(5, 65, "Loading...");
Show();
Sleep(500); //Just so they notice...
mode=2;
} break;
case 3: { //Save route
PlaySound(SOUND_BEEP);
SaveRoute();
ClearScreen();
SetTextColor(white);
DrawText(5, 65, "Saving...");
Show();
Sleep(500); //Just so they notice...
mode=2;
} break;
case 4: { //Recalibrate
PlaySound(SOUND_BEEP);
mode=3; //Recalibration mode
menuNum=1; //Use menuNum as steps throught the process - 1=ready, 2=working
} break;
case 5: { //About
PlaySound(SOUND_GO);
mode=5;
CloseMotors();
} break;
}
}
if(keystate & KEY_MENU) {
//Switch back to first menu
mode=1;
menuItem=1;
menuNum=1;
PlaySound(SOUND_BEEP);
}
} else if (mode==3) {
//Recalibrate.
if(keystate & KEY_INPUT1) {
//Cancel
PlaySound(SOUND_BEEP);
SetMotors(0,0);
mode=2;
menuItem=4;
menuNum=1;
}
if (keystate & KEY_INPUT2) {
//Start/Stop
PlaySound(SOUND_BEEP);
switch (menuNum) {
case 1: { //Start
menuNum=2;
ResetTimer();
SetMotors(10000,-10000);
}
break;
case 2: { //Stop
rotateSleep = ReadTimer() / 24;
SetMotors(0,0);
mode=2;
menuItem=4;
menuNum=1;
SaveCalibration();
}
}
}
} else if (mode==4 || mode==5) {
if(keystate & KEY_MENU) {
//Switch back to first menu
PlaySound(SOUND_BEEP);
mode=2;
menuItem=1;
menuNum=1;
OpenMotors();
}
if (keystate & KEY_INPUT1) {
IRState = !IRState;
SetIR(IRState);
}
if (keystate & KEY_INPUT2) {
PlaySound(SOUND_BEEP);
mode=2;
menuItem=1;
menuNum=1;
OpenMotors();
}
}
if(keystate & KEY_RUN) {
//Go
ClearScreen();
Show();
PlaySound(SOUND_GO);
//Cycle through steps and execute
int count;
SetTextColor(green);
for(count=1; count<=steps;count++) {
switch(moveList[count].cmd) {
case 1: { //Forward
ClearScreen();
DrawText(5, 100, "%d: Forward %d",count, moveList[count].num);
Show();
SetMotors(10000,10000);
Sleep(FORWARDSLEEP * moveList[count].num);
SetMotors(0,0);
}
break;
case 2: { //Back
ClearScreen();
DrawText(5, 100, "%d: Back %d",count, moveList[count].num);
Show();
SetMotors(-10000,-10000);
Sleep(FORWARDSLEEP * moveList[count].num);
SetMotors(0,0);
}
break;
case 3: { //Right
ClearScreen();
DrawText(5, 100, "%d: Right %d",count, moveList[count].num * 15);
Show();
SetMotors(10000,-10000);
Sleep(rotateSleep * moveList[count].num);
SetMotors(0,0);
}
break;
case 4: { //Left
ClearScreen();
DrawText(5, 100, "%d: Left %d",count, moveList[count].num * 15);
Show();
SetMotors(-10000,10000);
Sleep(rotateSleep * moveList[count].num);
SetMotors(0,0);
}
break;
case 5: { //Fire
ClearScreen();
SetTextColor(red);
DrawText(10, 45, "PEW! PEW! PEW!");
SetTextColor(green);
DrawText(5, 100, "%d: Fire %d",count, moveList[count].num);
Show();
int fireCount;
for(fireCount=0; fireCount<moveList[count].num; fireCount++) {
PlaySound(SOUND_FIRE);
}
}
}
}
//reset menu pointer
menuItem=1;
menuNum=1;
PlaySound(SOUND_GO);
}
DrawMenu();
}
Sleep(50); //to stop the radio being on full time
return true;
}