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;
}