void move_to_position(long setpoint)
{
pid_controller_t controller;
init_sonar_pid_controller(&controller);
pid_state_t state;
init_pid_state(&state, &controller);
while (abs(state.prev_error) > controller.CLOSE_ENOUGH)
{
long output = update_pid_controller(&controller, &state, setpoint, nMotorEncoder[sonar_rotate]);
motor[sonar_rotate] = output;
int reading = SensorValue[ultrasonic];
long encoder_reading = nMotorEncoder[sonar_rotate];
const int MIN_SONAR_DISTANCE = 30;
if (reading > MIN_SONAR_DISTANCE)
{
int bin = mod(encoder_reading / ENCODER_COUNTS_PER_BIN, SONAR_BINS);
sonar_readings[bin] = reading;
sonar_times[bin] = nPgmTime;
}
wait1Msec(10);
}
motor[sonar_rotate] = 0;
}
void move_motor_to_position(int motor_id, long desired_encoder, pid_controller_t* controller)
{
pid_state_t state;
init_pid_state(&state, controller);
while (abs(state.prev_error) > controller->CLOSE_ENOUGH)
{
long output = update_pid_controller(controller, &state, desired_encoder, nMotorEncoder[motor_id]);
motor[motor_id] = output;
wait1Msec(1);
}
motor[motor_id] = 0;
}
static void httpd_websocket_cb(struct altcp_pcb *pcb, uint8_t *data,
uint16_t data_len, uint8_t mode)
{
if (data_len == 0 || mode != WS_BIN_MODE) return;
uint8_t msgtype = data[0];
uint8_t *payload = &data[1];
data_len -= 1;
int8_t *signed_data;
pid_controller_index pid_index;
int32_t *i32_data;
uint8_t res;
switch (msgtype)
{
case STEERING:
// Parameters: velocity (int8_t), turn rate (int8_t)
if (data_len != 2) break;
signed_data = (int8_t *)payload;
set_steering((FLT_TO_Q16(SPEED_CONTROL_FACTOR) * signed_data[1]) / 128,
(FLT_TO_Q16(STEERING_FACTOR) * signed_data[0]) / 128);
break;
case REQ_ORIENTATION:
if (data_len != 0) break;
send_orientation(pcb);
break;
case REQ_SET_PID_PARAMS:
// Parameters: pid index (uint8_t), P (q16/int32_t), I (q16), D (q16)
if (data_len != 13) break;
pid_index = (pid_controller_index)payload[0];
i32_data = (int32_t *)(&payload[1]);
if (pid_index < (sizeof(pid_settings_arr) / sizeof(pid_settings_arr[0])))
{
update_pid_controller(pid_index, i32_data[0], i32_data[1],
i32_data[2]);
res = RES_SET_PID_PARAMS_ACK;
httpd_websocket_write(pcb, &res, 1, WS_BIN_MODE);
}
break;
case REQ_GET_PID_PARAMS:
if (data_len != 1) break;
if (payload[0] < (sizeof(pid_settings_arr) / sizeof(pid_settings_arr[0])))
{
send_pid_params(pcb, (pid_controller_index)payload[0]);
}
break;
case REQ_LOAD_FLASH_CONFIG:
if (data_len != 0) break;
load_config();
res = RES_LOAD_FLASH_CONFIG_DONE;
httpd_websocket_write(pcb, &res, 1, WS_BIN_MODE);
break;
case REQ_SAVE_CONFIG:
if (data_len != 0) break;
httpd_websocket_save_config(pcb);
break;
case REQ_CLEAR_CONFIG:
if (data_len != 0) break;
httpd_websocket_clear_config(pcb);
break;
}
}
task sonar_scanner()
{
nMotorEncoder[sonar_rotate] = 0;
pid_controller_t sonar_pid_controller;
init_sonar_pid_controller(&sonar_pid_controller);
pid_state_t sonar_pid_state;
init_pid_state(&sonar_pid_state, &sonar_pid_controller);
int sonar_destination = 0;
reset_readings();
while (1)
{
// keep moving the sonar
// turn the robot towards the closest object
long encoder_reading = nMotorEncoder[sonar_rotate];
long sonar_min = 0;
long sonar_max = 720;
sonar_pid_controller.MAX_OUTPUT = MAX_SONAR_MOTOR_SPEED;
int closest_bin = find_closest_sonar_bin();
switch (sonar_scan_mode)
{
case Sonar_Full:
if (sonar_readings[closest_bin] < 500 && have_recent_sonar_readings())
{
sonar_scan_mode = Sonar_Close;
}
else
{
sonar_min = 0;
sonar_max = 720;
if (sonar_destination != sonar_min && sonar_destination != sonar_max)
sonar_destination = sonar_min;
}
break;
case Sonar_Close:
if (sonar_readings[closest_bin] >= 500 || !have_recent_sonar_readings())
{
sonar_scan_mode = Sonar_Full;
}
else
{
sonar_min = (closest_bin - 2) * ENCODER_COUNTS_PER_BIN + 1;
sonar_max = (closest_bin + 2) * ENCODER_COUNTS_PER_BIN;
if (sonar_destination != sonar_min && sonar_destination != sonar_max)
sonar_destination = sonar_min;
}
break;
}
if (encoder_reading > sonar_min - sonar_pid_controller.CLOSE_ENOUGH && encoder_reading < sonar_min + sonar_pid_controller.CLOSE_ENOUGH)
sonar_destination = sonar_max;
else if (encoder_reading > sonar_max - sonar_pid_controller.CLOSE_ENOUGH && encoder_reading < sonar_max + sonar_pid_controller.CLOSE_ENOUGH)
sonar_destination = sonar_min;
writeDebugStreamLine("using sonar_destination: %d", sonar_destination);
long output = update_pid_controller(&sonar_pid_controller, &sonar_pid_state, sonar_destination, nMotorEncoder[sonar_rotate]);
motor[sonar_rotate] = output;
int reading = SensorValue[ultrasonic];
//writeDebugStreamLine("got sonar reading: %d", reading);
const int MIN_SONAR_DISTANCE = 30;
if (reading > MIN_SONAR_DISTANCE)
{
int bin = mod(encoder_reading / ENCODER_COUNTS_PER_BIN, SONAR_BINS);
sonar_readings[bin] = reading;
sonar_times[bin] = nPgmTime;
}
wait1Msec(1);
}
}
