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);
}
}
void setMotion(int fwdSpeed, int rotSpeed) {
// calculate individual motor speed
leftSpeed = fwdSpeed + rotSpeed;
rightSpeed = fwdSpeed - rotSpeed;
// set the direction
digitalWrite(MOTOR_LEFT_DIR, (leftSpeed > 0) ? LOW : HIGH);
digitalWrite(MOTOR_RIGHT_DIR, (rightSpeed < 0) ? LOW : HIGH);
// cap the speed
if (leftSpeed < -255)
leftSpeed = -255;
else if (leftSpeed > 255)
leftSpeed = 255;
if (rightSpeed < -255)
rightSpeed = -255;
else if (rightSpeed > 255)
rightSpeed = 255;
// set it to get full power for a fixed time
analogWrite(MOTOR_LEFT_PWM, 255);
analogWrite(MOTOR_RIGHT_PWM, 255);
// set a timer so we know when to revert to actual level
TMRArd_InitTimer(MOTOR_TIMER, MOTOR_FULL_POWER_TIME);
}
void stopMotion() {
// reverse direction for short time
digitalWrite(MOTOR_LEFT_DIR, (leftSpeed < 0) ? LOW : HIGH);
digitalWrite(MOTOR_RIGHT_DIR, (rightSpeed > 0) ? LOW : HIGH);
analogWrite(MOTOR_LEFT_PWM, 255);
analogWrite(MOTOR_RIGHT_PWM, 255);
leftSpeed = 0;
rightSpeed = 0;
// set stop timer
TMRArd_InitTimer(MOTOR_TIMER, MOTOR_STOP_TIME);
}
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;
}
void startLineFollowing(int spd) {
oldError = 0;
setMotion(spd, 0);
followLine(spd);
TMRArd_InitTimer(LINE_FOLLOW_TIMER, LINE_FOLLOW_UPDATE_PERIOD);
}
void start_pulse_timer(int delay_length){
// starts the pulse timer
TMRArd_InitTimer(PULSE_TIMER_ID, delay_length);
}
void start_timer(unsigned char timer_id, int duration){
TMRArd_InitTimer(timer_id, duration);
}