void ShieldGSM::executeATCommands(struct ATcommand *commandsList, int numCommands) { for (int i = 0; i < numCommands; i++) { struct ATcommand atc = commandsList[i]; int tocount=0; int state = INIT_WAIT_CODE; TMRArd_InitTimer(0, INIT_TIME); Serial.print(i); executeUserCommand(atc.cmd.c_str()); do { state = printShieldGSMResponse(atc.resp); if (state == INIT_WAIT_CODE && TMRArd_IsTimerExpired(0)) { state = INIT_TIMEOUT_CODE; TMRArd_InitTimer(0, INIT_TIME); } else if (state != INIT_WAIT_CODE) { TMRArd_InitTimer(0, INIT_TIME); } switch(state) { case INIT_TIMEOUT_CODE: Serial.println("COMMAND TIMEOUT: "+String(++tocount)); // intentionally no break case INIT_ERROR_CODE: executeUserCommand(atc.cmd.c_str()); break; default: break; } if(tocount>=3){ executeUserCommand(CTRL_Z.c_str()); return; } } while(state != INIT_SUCCESS_CODE); } }
// called periodically by main code to update motor state void updateMotor() { if (TMRArd_IsTimerExpired(MOTOR_TIMER) == TMRArd_EXPIRED) { // set the motor values to the actual power levels analogWrite(MOTOR_LEFT_PWM, abs(leftSpeed)); analogWrite(MOTOR_RIGHT_PWM, abs(rightSpeed)); } }
unsigned char respond_to_timer(unsigned char timer_id, unsigned char new_state){ if (TMRArd_IsTimerExpired(timer_id)){ change_state_to(new_state); return true; } else{ return false; } }
unsigned char state_init_timer_finished(){ if (state_init_finished) return false; // only call once unsigned char result = TMRArd_IsTimerExpired(STATE_INIT_TIMER); if (result){ state_init_finished = true; return true; } else { return false; } }
char followLine(int spd) { if (TMRArd_IsTimerExpired(LINE_FOLLOW_TIMER) != TMRArd_EXPIRED) return LINE_FOLLOW_OK; // reinit the timer TMRArd_InitTimer(LINE_FOLLOW_TIMER, LINE_FOLLOW_UPDATE_PERIOD); int val[3]; char retVal; if (spd > 0) readFrontSensors(val); else readBackSensors(val); int min = removeMin(val); int max = getMax(val); int correction = 0; int error = getLinePos(val); // check min and max to determine // where all sensors covered, or all sensors on white if (min > LINE_SENSOR_MIN_THRES) { // all black retVal = LINE_FOLLOW_ALL_LINE; } else if (max < LINE_SENSOR_MAX_THRES) { // no line! retVal = LINE_FOLLOW_NO_LINE; } else { correction = (error * LINE_KP) >> 3; correction += (error - oldError) * LINE_KD; retVal = LINE_FOLLOW_OK; } if (spd > 0) adjustMotion(spd, -correction); else adjustMotion(-spd, -correction); oldError = error; return retVal; }
unsigned char pulse_timer_finished(){ return TMRArd_IsTimerExpired(PULSE_TIMER_ID); }
char motorDoneStop() { return TMRArd_IsTimerExpired(MOTOR_TIMER) == TMRArd_EXPIRED; }