void Robot::setBrakesToCurrentRobotState()
{
// This method is private, so it does not need a mutex-lock. The callers should have taken care of that.
if(currentRobotStatus == idle || currentRobotStatus == stalled)
{
// The robot is NOT moving, apply the brakes.
setBrakes(true);
}
else if(currentRobotStatus == moving)
{
// The robot IS moving, release the brakes.
setBrakes(false);
}
else if(currentRobotStatus == aborting || currentRobotStatus == movementSetup)
{
// If we're entering one of these states, we need not change the brake-status. Why?
// For Abortion:
// - if we WERE moving before, we're now slowing down, so we still need the brakes loose.
//
// - if we were NOT moving, we're not going to start moving in the aborting state, so we
// can leave the brakes applied.
// For movementSetup, we only go there from an idle state, so the brakes are applied already.
logger->Robot("Robot::setBrakesToCurrentRobotState(): NOT changing brake-status, as we've entered aborting or movementSetup-state.");
}
}
__interrupt void interrupt1() {
if (P1IFG & I1_LEFT_BUTTON) {
switch (turnState) {
case NO_TURN:
leftTurn();
autoCancelState = AUTO_CANCEL_DISABLED;
break;
case LEFT_TURN:
cancelTurn();
break;
case RIGHT_TURN:
cancelTurn();
break;
}
P1IFG &= ~I1_LEFT_BUTTON; // Clear interrupt flag
}
if (P1IFG & I1_LEFT_BRAKE || P1IFG & I1_RIGHT_BRAKE) {
switch (brakeState) {
case NO_BRAKES:
setBrakes();
break;
}
P1IFG &= ~(I1_LEFT_BRAKE + I1_RIGHT_BRAKE); // Clear interrupt flag
}
LPM4_EXIT;
}
//magnitude of brake level reading
void __attribute__((__interrupt__, no_auto_psv)) _IC3Interrupt(void)
{
if(magnitude)
{
in2 = IC3BUF;
IC3CON2bits.TRIGSTAT = 0;
IC3CON2bits.TRIGSTAT = 1;
}
else if(!magnitude)
{
in2 = IC3BUF;
IC3CON2bits.TRIGSTAT = 0;
IC3CON2bits.TRIGSTAT = 1;
if(in2 > 22000)
{
brakeMag = 0;
}
else if(in2 > 20850)
{
brakeMag = 100;
}
else if((in2 < 14700) && (in2 > 0))
{
brakeMag = 0;
}
else
{
brakeMag = 4 * (unsigned long)in2 / 249 - 235;
}
setBrakes();
}
_IC3IF = 0;
}
//steering input
//MIGHT need to change when left and right go to max settings - right now left hits max way sooner than the right side
void __attribute__((__interrupt__, no_auto_psv)) _IC1Interrupt(void)
{
if(steer)
{
in1 = IC1BUF;
IC1CON2bits.TRIGSTAT = 0;
IC1CON2bits.TRIGSTAT = 1;
}
else if(!steer)
{
in1 = IC1BUF;
IC1CON2bits.TRIGSTAT = 0;
IC1CON2bits.TRIGSTAT = 1;
if((in1 > 33360) && (in1 < 50000)) //go full left
{
left = 100;
right = 0;
LED3 = 0;
}
else if((in1 < 27280) && (in1 > 0)) //go full right
{
right = 100;
left = 0;
LED3 = 0;
}
else if((in1 > 30387) && (in1 < 30487)) //go straight
{
right = 100;
left = 100;
LED3 = 0;
}
else
{
brakeSteer = (20 * (unsigned long)in1 / 1217) - 448;
LED3 = 1;
if((brakeSteer > 0) && (brakeSteer < 50))
{
left = brakeSteer * 2;
right = 100;
}
else if((brakeSteer > 50) && (brakeSteer < 100))
{
left = 100;
right = 2 * (100 - brakeSteer);
}
}
setBrakes();
}
_IC1IF = 0;
}
int main(void) {
WDTCTL = WDTPW | WDTHOLD; // Stop watchdog timer
// Clock Settings -----------------
BCSCTL1 = XT2OFF; // Disable XT2
BCSCTL2 = SELM1 + SELM0 + DIVM1 + DIVM0 + SELS + DIVS1 + DIVS0; // Set all clocks to use VLOCLK / 8
BCSCTL3 = LFXT1S1;
P1SEL = 0X00;
P2SEL = 0X00;
// Port Settings ------------------------
P1DIR = O1_LEFT_LIGHT + O1_BRAKE_LIGHT + O1_DEBUG_LIGHT; // Set outputs
P1REN = I1_LEFT_BRAKE + I1_RIGHT_BRAKE + I1_LEFT_BUTTON; // Enable pull up resistors
P1OUT = I1_LEFT_BRAKE + I1_RIGHT_BRAKE + I1_LEFT_BUTTON; // Set pull up resistors
P1OUT &= ~(O1_LEFT_LIGHT + O1_BRAKE_LIGHT); // Turn off lights
P1IE = I1_LEFT_BUTTON + I1_LEFT_BRAKE + I1_RIGHT_BRAKE; // Set interrupt sources
P1IES &= ~(I1_LEFT_BUTTON + I1_LEFT_BRAKE + I1_RIGHT_BRAKE); // Interrupt on rising edge
P2DIR = O2_RIGHT_LIGHT; // Set outputs
P2REN = I2_RIGHT_BUTTON; // Enable pull up resistors
P2OUT = I2_RIGHT_BUTTON; // Enable pull up resistors
P2OUT &= ~(O2_RIGHT_LIGHT); // Turn off lights
P2IE = I2_RIGHT_BUTTON; // Set interrupt sources
P2IES &= ~(I2_RIGHT_BUTTON); // Interrupt on rising edge.
// Timer A settings
TACCR0 = 600;
TACCR1 = 300;
TACTL = TASSEL1; // Use SMCLK
TACCTL1 = OUTMOD2 + OUTMOD1 + OUTMOD0 + CCIE; // Reset/Set mode, enable interrupts
// ADC settings
ADC10CTL0 &= ~(ENC); // Disable ADC before setting up
// 8 cycle sample/hold, use 2.5V internal reference, use slower sampling rate, reference burst (power save)
ADC10CTL0 = SREF0 + ADC10SHT0 + ADC10SR + REFBURST + REF2_5V + REFON + ADC10ON;// + ADC10IE;
ADC10CTL1 = ADC10SSEL1 + ADC10SSEL0; // Run off SMCLK
ADC10AE0 = I1_COMPASS_A + I1_COMPASS_B;
turnState = NO_TURN;
brakeState = NO_BRAKES;
compassState = COMPASS_A;
autoCancelState = AUTO_CANCEL_DISABLED;
_EINT(); // Global enable interrupts
LPM4;
while (1) {
// Power down logic
if (brakeState == NO_BRAKES && turnState == NO_TURN) {
// Check that we haven't missed a brake signal before going to sleep
if (brakesActive()) {
//setBrakes();
}
else {
_EINT();
LPM4; // Enter low power mode until a brake or turn event
}
}
// Braking logic
if (brakeState == BRAKES) {
if(brakesActive()) {
// currently braking, do nothing
}
else {
// stop braking
cancelBrakes();
}
}
// Check that brakes aren't active, and turn on light if necessary
else {
if(brakesActive())
setBrakes();
}
// Entry condition: in a turn state AND ADC not busy AND no ADC interrupts pending
if (turnState != NO_TURN && !(ADC10CTL1 & ADC10BUSY) && !(ADC10CTL0 & ADC10IFG)) {
// Start the ADC
flush();
startAnalogRead();
}
// Entry condition: ADC data ready AND in a turn state
if ((ADC10CTL0 & ADC10IFG) && turnState != NO_TURN) {
// Put data in buffer
genericCompassValue = ADC10MEM;
// Clear interrupt flag before proceeding
ADC10CTL0 &= ~(ADC10IFG);
bool bufferOverflow = push(&genericCompassValue);
if (bufferOverflow) {
switch(compassState) {
case COMPASS_A:
averagedCompassValueA = getAverage();
compassState = COMPASS_B;
flush();
startAnalogRead();
break;
case COMPASS_B:
averagedCompassValueB = getAverage();
compassState = COMPASS_A;
flush();
startAnalogRead();
break;
}
// We need to pass through two buffer overflows to get current compass data from both
// channels. On the first pass we set the state to initializing, and on the second pass
// we set the state to ready.
switch (autoCancelState) {
case AUTO_CANCEL_DISABLED:
autoCancelState = AUTO_CANCEL_INITIALIZING;
case AUTO_CANCEL_INITIALIZING:
autoCancelState = AUTO_CANCEL_READY;
break;
}
}
// Keep polling compass
else {
startAnalogRead();
}
// Debug test B-670
if(averagedCompassValueA > 650)
P1OUT |= O1_DEBUG_LIGHT;
else
P1OUT &= ~O1_DEBUG_LIGHT;
// end debug
}
// Entry condition: in a turn state AND auto cancel is active
// In here we have auto-cancel logic
if (turnState != NO_TURN && autoCancelState == AUTO_CANCEL_ACTIVE) {
int absTurnA = startCompassValueA - averagedCompassValueA;
int absTurnB = startCompassValueB - averagedCompassValueB;
absTurnA = (absTurnA >= 0) ? absTurnA : -absTurnA;
absTurnB = (absTurnB >= 0) ? absTurnB : -absTurnB;
if (absTurnA + absTurnB > MIN_TURN_VALUE) {
cancelTurn();
autoCancelState = AUTO_CANCEL_DISABLED;
}
}
}
return 0;
}
