void step() {
switch(state) {
case ACTIVATE_PENDING:
if ((get_time() - state_time) > 750) {
state_time = get_time();
state = ACTIVATED;
}
break;
case ACTIVATED:
updateSelfPosition(true);
determineTargetPosition();
updateAngleToTarget();
state = NAVIGATE;
resetPID(angle_to_target);
state_time = get_time();
break;
case NAVIGATE:
navigateToTarget();
break;
default:
//rf_printf("Default\n");
break;
}
//update_pid(&driver);
//pause(50);
}
bool navigateToTarget() {
//return true if need to be called next frame
if (get_time() - state_time > 2800) {
determineTargetPosition();
updateSelfPosition(true);
updateAngleToTarget();
state_time = get_time();
}
else if (get_time() - state_time > 2500) {
motor_set_vel(RIGHT_MOTOR,0);
motor_set_vel(LEFT_MOTOR,0);
return true;
}
if (get_time() - last_update_time > 100) {
updateSelfPosition(false);
determineTargetPosition();
updateAngleToTarget();
}
if (getDistanceToTarget(target_x,target_y) < 4.0) {
motor_set_vel(RIGHT_MOTOR,0);
motor_set_vel(LEFT_MOTOR,0);
resetPID(0);
return false;
}
update_pid(&driver);
return true;
}
/**
* The main setup function for the vehicle. Initalized the Amarino communications,
* then calls the various setup functions for the various modules.
*/
void setup()
{
// Core board initialization
initBoard();
// Arm thruster
thruster.arm();
// Reset all PID values to zero
resetPID();
// Load PID constants in from EEPROM
eeprom_read_block(&pid, &pidEeprom, sizeof(pidConstants_t));
// Set up serial communications
amarino.registerFunction(setVelocity, SET_VELOCITY_FN);
amarino.registerFunction(setPID, SET_PID_FN);
amarino.registerFunction(getPID, GET_PID_FN);
// Configure ADC
setupADC();
}
/* Enter the main loop. Read and parse input from the serial port
and run any valid commands. Run a PID calculation at the target
interval and check for auto-stop conditions.
*/
void loop() {
while (Serial.available() > 0) {
// Read the next character
chr = Serial.read();
// Terminate a command with a CR
if (chr == 13) {
if (arg == 1) argv1[index] = NULL;
else if (arg == 2) argv2[index] = NULL;
runCommand();
resetCommand();
}
// Use spaces to delimit parts of the command
else if (chr == ' ') {
// Step through the arguments
if (arg == 0) arg = 1;
else if (arg == 1) {
argv1[index] = NULL;
arg = 2;
index = 0;
}
continue;
}
else {
if (arg == 0) {
// The first arg is the single-letter command
cmd = chr;
}
else if (arg == 1) {
// Subsequent arguments can be more than one character
argv1[index] = chr;
index++;
}
else if (arg == 2) {
argv2[index] = chr;
index++;
}
}
}
// If we are using base control, run a PID calculation at the appropriate intervals
#ifdef USE_BASE
if (millis() > nextPID) {
updatePID();
nextPID += PID_INTERVAL;
}
// Check to see if we have exceeded the auto-stop interval
if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) {;
setMotorSpeeds(0, 0);
moving = 0;
resetPID();
}
#endif
if (millis() > nextTick) {
ticks += 1;
nextTick += TICK_INTERVAL;
}
}
/* Run a command. Commands are defined in commands.h */
int runCommand() {
int i = 0;
char *p = argv1;
char *str;
int pid_args[4];
arg1 = atoi(argv1);
arg2 = atoi(argv2);
switch(cmd) {
case GET_TICKS:
Serial.println(ticks);
break;
case GET_BAUDRATE:
Serial.println(BAUDRATE);
break;
case ANALOG_READ:
Serial.println(analogRead(arg1));
break;
case DIGITAL_READ:
Serial.println(digitalRead(arg1));
break;
case ANALOG_WRITE:
analogWrite(arg1, arg2);
Serial.println("OK");
break;
case DIGITAL_WRITE:
if (arg2 == 0) digitalWrite(arg1, LOW);
else if (arg2 == 1) digitalWrite(arg1, HIGH);
Serial.println("OK");
break;
case PIN_MODE:
if (arg2 == 0) pinMode(arg1, INPUT);
else if (arg2 == 1) pinMode(arg1, OUTPUT);
Serial.println("OK");
break;
case PING:
Serial.println(Ping(arg1));
break;
#ifdef USE_SERVOS
case SERVO_WRITE:
servos[arg1].write(arg2);
Serial.println("OK");
break;
case SERVO_READ:
Serial.println(servos[arg1].read());
break;
#endif
#ifdef USE_BASE
case READ_ENCODERS:
Serial.print(readEncoder(LEFT));
Serial.print(" ");
Serial.println(readEncoder(RIGHT));
break;
case RESET_ENCODERS:
resetEncoders();
Serial.println("OK");
break;
case READ_MOTORS:
Serial.print(leftPID.output);
Serial.print(" ");
Serial.println(rightPID.output);
break;
case MOTOR_SPEEDS:
/* Reset the auto stop timer */
lastMotorCommand = millis();
if (arg1 == 0 && arg2 == 0) {
setMotorSpeeds(0, 0);
moving = 0;
resetPID();
}
else moving = 1;
leftPID.TargetTicksPerFrame = arg1;
rightPID.TargetTicksPerFrame = arg2;
Serial.println("OK");
break;
case UPDATE_PID:
while ((str = strtok_r(p, ":", &p)) != '\0') {
pid_args[i] = atoi(str);
i++;
}
Kp = pid_args[0];
Kd = pid_args[1];
Ki = pid_args[2];
Ko = pid_args[3];
Serial.println("OK");
break;
#endif
default:
Serial.println("Invalid Command");
break;
}
}
