static inline void
control_thums() {
switch (control_configuration) {
case ALTITUDE_FROM_POWER:
control.throttle = pid_update(&throttle_pid,
altitude_ref - attitude.altitude);
pitch_ref = pid_update(&pitch_pid,
airspeed_ref - attitude.airspeed);
break;
case ALTITUDE_FROM_PITCH:
pitch_ref = pid_update(&pitch_pid,
altitude_ref - attitude.altitude);
control.throttle = pid_update(&throttle_pid,
airspeed_ref - attitude.airspeed);
break;
}
roll_ref = pid_update(&roll_pid,
yaw_ref - attitude.att_est[2]);
control.aileron = pid_update(&aileron_pid,
roll_ref - attitude.att_est[0]);
control.elevator = pid_update(&elevator_pid,
pitch_ref - attitude.att_est[1]);
control.elevator += fabs(roll_ref) * elevator_ff;
control.rudder = pid_update(&rudder_pid, - sensor.acc[1]);
control.rudder += control.aileron * rudder_ff;
}
void PID_Update()
{
#ifndef PID_BYPASS
static uint32 last_time = 0;
static int16 new_velocity = 0;
int16 velocity;
int16 input = 0;
uint32 delta_time;
/* Read the velocity command */
I2c_ReadVelocity(&velocity);
delta_time = millis() - last_time;
//if (DELTA_TIME(millis(), last_time, SAMPLE_PERIOD))
if (delta_time > SAMPLE_PERIOD)
{
last_time = millis();
input = Encoder_GetVelocity();
pid_update(&pid, velocity - input);
new_velocity = input + pid.control;
PrintPid(&pid, new_velocity);
Motor_SetOutput(new_velocity);
}
#else
Motor_SetOutput(velocity);
#endif
}
static void speed_control_algorithm(uckernel_task_event event, uckernel_task_data data)
{
int16_t result;
result = pid_update(&speed_control_pid, measured_speed);
set_servo_duty(result);
if(is_running)
uckernel_submit_timed_task(speed_control_algorithm, NULL, NULL, speed_control_pid.interval);
}
/* -----------------------------------------------------------------------------
* Function: state_comp_forward(control_variables * local_state_vars_ptr);
*
* Drives forward using compass as angular feedback.
*
* INPUTS: local_state_vars
*
* OUTPUTS: none
*/
void state_comp_forward(control_variables * local_state_vars_ptr)
{
// pid setpoint (desired angle) is set in structure when initialised
// pid_update calculates a new control variable and writes it into struct
pid_update(local_state_vars_ptr->compass_currentheading,
local_state_vars_ptr->pid_ctrl_ptr);
pid_updatemotors_fwd(local_state_vars_ptr);
}
float stabilize_from_angle(struct PIDState* n_var0, const
struct PIDConfig* n_var1, struct PIDState* n_var2,
const struct PIDConfig* n_var3, float n_var4,
float n_var5, float n_var6, float n_var7,
float n_var8)
{
REQUIRES(n_var8 != 0.0f);
float n_let0 = n_var4 * n_var5;
float n_let1 = n_var6 * 57.29578f;
float n_let2 = n_let0 - n_let1;
float n_r3 = pid_update(n_var0, n_var1, n_let2, n_let1);
float n_let4 = n_var7 * 57.29578f;
float n_let5 = n_r3 - n_let4;
float n_r6 = pid_update(n_var2, n_var3, n_let5, n_let4);
float n_r7 = fconstrain(-n_var8, n_var8, n_r6);
ASSERTS(n_var8 != 0.0f);
return n_r7 / n_var8;
}
/* -----------------------------------------------------------------------------
* Function: state_comp_reverse(control_variables * local_state_vars_ptr)
*
* Reverse in a straight line using the compass as angular feedback.
* Use a forward heading for the PID, but set the motors in reverse.
* Then, the steering inputs are reversed.
*
* INPUTS: local_state_vars_ptr
*
* OUTPUTS: none
*/
void state_comp_reverse(control_variables * local_state_vars_ptr)
{
// pid setpoint (desired angle) is set in structure when initialised
// pid_update calculates a new control variable and writes it into struct
pid_update(local_state_vars_ptr->compass_currentheading,
local_state_vars_ptr->pid_ctrl_ptr);
pid_updatemotors_rev(local_state_vars_ptr);
// generate break condition if error is small.
}
/* -----------------------------------------------------------------------------
* Function: state_comp_rev_right(control_variables * local_state_vars_ptr)
*
* Reverse to the right 90 degrees, and use the compass as angular feedback.
*
* INPUTS: local_state_vars_ptr
*
* OUTPUTS: none
*/
void state_comp_rev_right(control_variables * local_state_vars_ptr)
{
// pid setpoint (desired angle) is set in structure when initialised
// pid_update calculates a new control variable and writes it into struct
pid_update(local_state_vars_ptr->compass_currentheading,
local_state_vars_ptr->pid_ctrl_ptr);
// update motors
L_motor_acceltoconstSpeed(local_state_vars_ptr->update_counter,
REV,
(local_state_vars_ptr->motor_dualspeed + local_state_vars_ptr->Controller1.cv));
}
/* Control the flight of the helicopter. */
static void vPIDControlHeli( void *pvParameters)
{
//Set the altitude gains.
static const float Aki = 0.4, Akp = 3.0, Akd = 0.25;
static const float delta_t = 0.02;
float altError, altResponse;
float currentAltitude = 0;
portBASE_TYPE xStatus;
int buttonMessage;
xAltMessage altMessage;
xQueueHandle xOLEDQueue = ( xQueueHandle ) pvParameters;
portTickType xLastWakeTime = xTaskGetTickCount();
/* Define a period of 2 milliseconds (500Hz) */
const portTickType xPeriod = ( 3 / portTICK_RATE_MS );
for ( ;; ) {
vTaskDelayUntil( &xLastWakeTime, xPeriod );
// Act on button presses sent from the ISR.
xStatus = xQueueReceive( xButtonQueue, &buttonMessage, 0 );
if (xStatus == pdPASS)
{
switch (buttonMessage)
{
case UP:
if (desiredAltitude <= 80)
desiredAltitude += 10;
break;
case DOWN:
if (desiredAltitude >= 10)
desiredAltitude -= 10;
break;
}
xQueueMessage message = { DESIRED_ALTITUDE, desiredAltitude };
xQueueSendToBack( xOLEDQueue, &message, 0 );
}
// Recompute the PID output using the latest altitude value.
xStatus = xQueueReceive( xAltQueue, &altMessage, 0 );
if (xStatus == pdPASS)
{
currentAltitude = altMessage.pcMessage;
altError = desiredAltitude - currentAltitude;
altResponse = pid_update(altError, Akp, Aki, Akd, delta_t);
PWMPulseWidthSet(PWM_BASE, PWM_OUT_1, ( SysCtlClockGet() / 150 ) * altResponse / 100);
}
}
}
float stabilize_from_rate(struct PIDState* n_var0, const
struct PIDConfig* n_var1, float n_var2, float n_var3,
float n_var4, float n_var5)
{
REQUIRES(n_var5 != 0.0f);
float n_let0 = n_var2 * n_var3;
float n_let1 = n_var4 * 57.29578f;
float n_let2 = n_let0 - n_let1;
float n_r3 = pid_update(n_var0, n_var1, n_let2, n_let1);
float n_r4 = fconstrain(-n_var5, n_var5, n_r3);
ASSERTS(n_var5 != 0.0f);
return n_r4 / n_var5;
}
/* -----------------------------------------------------------------------------
* Function: state_psns_forward(control_variables * local_state_vars_ptr);
*
* Moves the robot forward, while using the photosensors to estimate
* the angular deviation, and a PID controller to correct the error.
*
* INPUTS: local_state_vars_ptr
*
* OUTPUT: none
*/
void state_psns_forward(control_variables * local_state_vars_ptr)
{
// ADC sample data is already in buffer
signed int diffs[2] = {0};
signed int * diffs_ptr = diffs;
// calculate two differences
linefollow_calcpairdiffs(local_state_vars_ptr->psns_adc_samples, diffs_ptr);
// estimate angle from differences
signed int psns_estimate_angle = 0;
psns_estimate_angle = linefollow_estimateangle_ldr(diffs_ptr);
// input new angular error estimate into PID controller
pid_update(psns_estimate_angle,
local_state_vars_ptr->pid_ctrl_ptr);
// now update motors
pid_updatemotors_linefollow(local_state_vars_ptr);
}
//
// main loop, charging
// ===================
// During all loop (fast & constant charge) the charger check
// the temperature and if battery is hot unplugged
//
// 2)If OK control proceeds to the Constant Current loop where the PWM signal
// on-time is adjusted up or down until the measured current matches the
// specified fast charge current.
// -> The charge LED flashes quickly during this mode.
//
// 3)When the Maximum Charge Voltage setting is reached then the
// mode switches to Constant Voltage and a new loop is entered that uses
// the same method as above to control voltage at the Maximum Charge
// Voltage point.
// -> The LED now flashes more slowly.
//
// This continues until 30 minutes after the charge current, which is
// decreasing with time in constant voltage mode, reaches the minimum
// (set at 50 mA per 1600 mAH of capacity).
// ->The charger then shuts off and the LED goes on steady.
//
//
int charger_mainloop(CHARGER& charger){
long charging=1, count=0, constant_voltage=false;
double p_norm=P_NORM, i_norm=I_NORM;
//
// one loop is about (on arduino 16Mhz)
// - 920us, 1087Hz (1x avg_read(3 iter) without Serial.print)
// - 1.8ms, 555Hz (2x avg_read without Serial.print=
// - 3.3ms, 306Hz (only with Serial.print)
while(charging){
digitalWrite(P_SW,HIGH);
// only peak temp every (1.8 - 5.0 ms *1000 => ~2-5 seconds )
if((charging++)%400==0){
//charger.temp=avr_internal_temp();
charging=1;
if (constant_voltage)Serial.print("CST ");
Serial.print("INFO V: ");Serial.print(charger.vfb*V_FACTOR,2);
Serial.print(" PWM: ");Serial.print((int)(charger.pwm));
//Serial.print(" SP: ");Serial.print(A2D(V_BATT));
Serial.print(" TEMP: ");Serial.print(charger.temp);
Serial.print(" FB: ");Serial.print((int)(charger.fb));
Serial.print(" ERROR: ");Serial.print((int)(pid.e));
Serial.print(" P: ");Serial.println(charger.ifb*I_FACTOR*charger.vfb*V_FACTOR,2);
}
//
// read voltage feedback
charger.vfb=avg_read(A_VFB,charger.dfb_v);
charger.avg_vfb=charger.avg_vfb*.9+charger.vfb*.1;
charger.ifb=avg_read(A_IFB,charger.dfb_i);
#if 1
//
// detect over temperature
if(charger.temp>TEMP_MAX){
#ifdef CONFIG_WITH_PRINT
Serial.print("OVER TEMP: ");Serial.println(charger.temp,2);
#endif
charger_reset(charger,MIN_PWM);
while(true);
continue;
}
//
// check mosfet
charger_check_mosfet(charger);
#ifdef CONFIG_WITH_PRINT
//
// detect offline (FOR DEBUG ONLY)
if ((charger.vfb)<=V_IN && (charger.ifb<I_BATT_CHARGED) && count++>300){
Serial.print("OFF : ");
Serial.print(charger.vfb*V_FACTOR,2);
Serial.print(" PWM: ");
Serial.println((int)charger.pwm);
// re init pwm
charger_reset(charger,MIN_PWM);
delay(TIMER_WAIT);
return false;
}
#endif
//
// security trigger on over voltage
if (charger.vfb>O_V){
#ifdef CONFIG_WITH_PRINT
Serial.print("OVER V: ");Serial.print(charger.vfb*V_FACTOR,2);
Serial.print(" PWM: ");Serial.print((int)(charger.pwm));
Serial.print(" I: ");Serial.println(charger.ifb*I_FACTOR,2);
#endif
charger_reset(charger,MIN_PWM);
delay(TIMER_WAIT*3);
continue;
}
//
// security trigger on over current
if ((charger.ifb*charger.vfb)>(P_MAX*1.1)){
#ifdef CONFIG_WITH_PRINT
Serial.print("OVER P: ");Serial.print(charger.ifb*I_FACTOR*charger.vfb*V_FACTOR,2);
Serial.print(" PWM: ");Serial.print((int)(charger.pwm));
Serial.print(" V: ");Serial.println(charger.vfb*V_FACTOR,2);
#endif
charger_reset(charger,MIN_PWM);
delay(TIMER_WAIT*3);
continue;
}
//
// check if cherging is done
if (abs(charger.vfb-V_BATT)<=0.1 && charger.ifb<=I_BATT_CHARGED && charger.ifb>0.01){
#ifdef CONFIG_WITH_PRINT
Serial.print("CHARGED: V=");
Serial.print(charger.vfb*V_FACTOR,2);
Serial.print(" I=");Serial.println(charger.ifb*I_FACTOR,2);
#endif
charger_reset(charger,MIN_PWM);
while(true);
}
#endif
//
// limit on voltage for constant voltage
if ((V_BATT-charger.avg_vfb)<=0.01 || constant_voltage){
constant_voltage=true;
charger.pwm=pid_update(pid,charger.sp,charger.dfb_v);
}else{
//
// limit on power for fast charge
charger.fb=A2D(charger.ifb*charger.vfb);//charger.dfb_v;
charger.pwm=pid_update(pid,A2D(P_MAX),charger.fb);
}
analogWrite(P_PWM, (int)(charger.pwm) );
}
charger_reset(charger,MIN_PWM);
return true;
}
void controller_stable( void *ptr ) {
unsigned long c = 0;
float m_fl,m_bl,m_fr,m_br; //motor FL, BL, FR, BR
float loop_ms = 1000.0f/GYRO_RATE;
float loop_s = loop_ms/1000.0f;
while (ms_update()!=0);//empty MPU
for (int i=0;i<3;i++) {
pid_setmode(&config.pid_r[i],1);
pid_setmode(&config.pid_s[i],1);
}
flush();
t=rt_timer_read();
while(1) {
ms_err = ms_update();
if (ms_err==0) continue; //dont do anything if gyro has no new data; depends on gyro RATE
if (ms_err<0) { //something wrong with gyro: i2c issue or fifo full!
ms.ypr[0]=ms.ypr[1]=ms.ypr[2] = 0.0f;
ms.gyro[0]=ms.gyro[1]=ms.gyro[2] = 0.0f;
}
c++; //our counter so we know when to read barometer (c increases every 10ms)
//bs_err = bs_update(c*loop_ms);
rec_err = rec_update();
pre_flight();
handle_issues();
autoflight();
if (armed && rec.yprt[3]>(config.esc_min+10)) {
inflight = 1;
}
if (rec.yprt[3]<=(config.esc_min+10)) {
inflight = 0;
sc_update(MOTOR_FL,config.esc_min);
sc_update(MOTOR_BL,config.esc_min);
sc_update(MOTOR_FR,config.esc_min);
sc_update(MOTOR_BR,config.esc_min);
yaw_target = ms.ypr[0];
bs.p0 = bs.p;
}
if (rec.yprt[3]<(config.esc_min+50)) { //use integral part only if there is some throttle
for (int i=0;i<3;i++) {
config.pid_r[i]._KiTerm = 0.0f;
config.pid_s[i]._KiTerm = 0.0f;
}
}
do_adjustments();
//our quad can rotate 360 degree if commanded, it is ok!; learned it the hard way!
if (yaw_target-ms.ypr[0]<-180.0f) yaw_target*=-1;
if (yaw_target-ms.ypr[0]>180.0f) yaw_target*=-1;
//do STAB PID
for (int i=0;i<3;i++) {
if (i==0) //keep yaw_target
pid_update(&config.pid_s[i],yaw_target,ms.ypr[i],loop_s);
else
pid_update(&config.pid_s[i],rec.yprt[i]+config.trim[i],ms.ypr[i],loop_s);
}
//yaw requests will be fed directly to rate pid
if (abs(rec.yprt[0])>7.5f) {
config.pid_s[0].value = rec.yprt[0];
yaw_target = ms.ypr[0];
}
if (mode == 1) { //yaw setup
config.pid_s[0].value = 0.0f;
config.pid_s[1].value = ms.gyro[1];
config.pid_s[2].value = ms.gyro[2];
} else if (mode == 2) { //pitch setup
config.pid_s[0].value = ms.gyro[0];
config.pid_s[1].value = 0.0f;
config.pid_s[2].value = ms.gyro[2];
} else if (mode == 3) { //roll setup
config.pid_s[0].value = ms.gyro[0];
config.pid_s[1].value = ms.gyro[1];
config.pid_s[2].value = 0.0f;
}
//do RATE PID
for (int i=0;i<3;i++) {
pid_update(&config.pid_r[i],config.pid_s[i].value,ms.gyro[i],loop_s);
}
//calculate motor speeds
m_fl = rec.yprt[3]-config.pid_r[2].value-config.pid_r[1].value+config.pid_r[0].value;
m_bl = rec.yprt[3]-config.pid_r[2].value+config.pid_r[1].value-config.pid_r[0].value;
m_fr = rec.yprt[3]+config.pid_r[2].value-config.pid_r[1].value-config.pid_r[0].value;
m_br = rec.yprt[3]+config.pid_r[2].value+config.pid_r[1].value+config.pid_r[0].value;
log();
if (inflight) {
sc_update(MOTOR_FL,m_fl);
sc_update(MOTOR_BL,m_bl);
sc_update(MOTOR_FR,m_fr);
sc_update(MOTOR_BR,m_br);
}
}
}
int main(void)
{
//Set data direction, set mux pin to low and turn off LEDs.
DDRD |= _BV(SERIAL_MUX_PIN) | _BV(RED_LED_PIN) | _BV(BLUE_LED_PIN);
PORTD &= ~(_BV(SERIAL_MUX_PIN) | _BV(BLUE_LED_PIN) | _BV(RED_LED_PIN));
//Enable USART and connect to scanf/printf.
USART_init();
USART_to_stdio();
//Initialize sensors and FPGA.
mpu_init();
fpga_init(&PORTB, &DDRB, 2); //Needs to be after mpu_init().
mag_init();
milli_timer_init();
//This is pretty important.
sei();
//Get some initial readings
mpu_scale();
mag_read();
attitude();
mag_calculate(roll, pitch);
//Use those to initialize PID state
pid_init(&pitch_pid, P_CONST, I_CONST, D_CONST, pitch + PITCH_CORRECT);
pid_init(&roll_pid, P_CONST, I_CONST, D_CONST, roll + ROLL_CORRECT);
pid_init(&heading_pid, P_CONST, I_CONST, D_CONST, heading);
//We want to be level and retain heading (for now)
target_pitch = 0;
target_roll = 0;
target_heading = heading;
//Calibrate and arm the motors.
printf("Enter y to calibrate and arm motors.\n");
scanf("%c", &cmd);
if (cmd == 'y')
{
calibrate_motors();
printf("Motors calibrated and armed.\n");
}
else
{
printf("Calibration skipped.\n");
do
{
printf("Enter 'a' to arm motors.\n");
scanf("%c", &cmd);
} while (cmd != 'a');
arm_motors();
PORTD |= _BV(BLUE_LED_PIN);
printf("Motors armed.\n");
}
uint16_t loop_duration;
unsigned long last_loop_time = 0;
uint8_t count = 0;
while(1)
{
//unsigned pt = PID_PERIOD;
//unsigned pf = PID_FREQ;
/*
printf("Period: %u; frequency: %u\n", pt, pf);
//Wait for pid flag to be true.
while (!pid_flag);
ATOMIC_BLOCK(ATOMIC_RESTORESTATE) //Reset it atomically as soon as we start.
{
pid_flag = 0;
}
*/
loop_duration = current_time - last_loop_time;
last_loop_time = current_time;
mpu_scale();
mag_read();
attitude();
//mag_calculate(roll, pitch);
pitch_correct = CORRECTION_GAIN * pid_update(&pitch_pid, target_pitch, pitch + PITCH_CORRECT);
roll_correct = CORRECTION_GAIN * pid_update(&roll_pid, target_roll, roll + ROLL_CORRECT);
//heading_correct = CORRECTION_GAIN * pid_update(&heading_pid, target_heading, heading);
heading_correct = 0;
//Safety: if throttle=0, stop motors.
if (throttle)
{
PORTD &= ~_BV(RED_LED_PIN);
//adjust_attitude(pitch_correct, roll_correct, heading_correct);
pcms[0] = throttle; pcms[1] = throttle; pcms[2] = throttle; pcms[3] = throttle;
set_pcm(pcms);
}
else
{
PORTD |= _BV(RED_LED_PIN);
pcms[0] = 0; pcms[1] = 0; pcms[2] = 0; pcms[3] = 0;
set_pcm(pcms);
//prevent integration windup when stationary
roll_pid.integrated_error = 0;
pitch_pid.integrated_error = 0;
}
++count;
#ifdef PRINTING
if (count % 100 == 0)
{
#ifdef PRINT_CSV
printf("%i,%i,%i,", accel_x, accel_y, accel_z);
printf("%f,%f,%f,", gyro_x, gyro_y, gyro_z);
printf("%i,%i,%i,", mag_x, mag_y, mag_z);
printf("%f,%f,%f,", pitch, roll, heading);
printf("%f,%f,%f,", pitch_correct, roll_correct, heading_correct);
printf("%u,%u,%u,%u,%u", (0x00FF & (unsigned)pcms[0]), (0x00FF & (unsigned)pcms[1]), (0x00FF & (unsigned)pcms[2]), (0x00FF & (unsigned)pcms[3]), (0x00FF & (unsigned)throttle));
printf("%u\n", loop_duration);
#endif
#ifdef PRETTY_PRINT
printf("ax: %i; ay: %i; az: %i; ", accel_x, accel_y, accel_z);
printf("gx: %f; gy: %f; gz: %f; ", gyro_x, gyro_y, gyro_z);
printf("mx: %i; my: %i; mz: %i; ", mag_x, mag_y, mag_z);
printf("p: %f; r: %f; h: %f; ", pitch + PITCH_CORRECT, roll + ROLL_CORRECT, heading_deg);
printf("PC: %f; RC: %f; HC: %f; ", pitch_correct, roll_correct, heading_correct);
printf("fl: %u; fr: %u; rl: %u; rr: %u; ", (0x00FF & (unsigned)pcms[FRONT_LEFT]), (0x00FF & (unsigned)pcms[FRONT_RIGHT]), (0x00FF & (unsigned)pcms[REAR_LEFT]), (0x00FF & (unsigned)pcms[REAR_RIGHT]));
printf("thr: %u; ", (0x00FF & (unsigned)throttle));
printf("t: %u\n", loop_duration);
#endif
}
#endif
//don't send anything other than numbers, dumbass
if (!(count % 64) && (rx_bytes >= 2))
{
if (scanf("%u", &num) != 0x00)
{
if (num > 800)
throttle = 800;
else
throttle = num;
#ifndef PRINTING
printf("Throttle: %u\n", throttle);
#endif
}
}
}
}
void navigation_update_state()
{
int32_t distance_values[N_ULTRASONIC_SENSORS];
int32_t smallest = 0;
int32_t left_s = 0;
int32_t right_s = 0;
int32_t front1_s = 0;
int32_t front2_s = 0;
ultrasonic_get_distance(distance_values);
smallest = ultrasonic_get_smallest(distance_values, N_ULTRASONIC_SENSORS);
left_s = distance_values[US_LEFT];
right_s = distance_values[US_RIGHT];
front1_s = distance_values[US_FRONT_1];
front2_s = distance_values[US_FRONT_2];
if(navigation_status.current_state == BYPASS){
//motor command is done directly in CLI
}
else if(navigation_status.current_state == AVOID_OBSTACLE){
if (smallest > navigation_status.max_dist_obs){
navigation_status.current_state = GO_TO_TARGET;
GPIO_write(Board_OBS_A_EN, 0);
}else if ((left_s < BACKWARD_THRESHOLD/2) || (right_s < BACKWARD_THRESHOLD/2)){
navigation_status.current_state = SPACE_NEEDED;
}else if (front1_s < BACKWARD_THRESHOLD && front2_s < BACKWARD_THRESHOLD){
serial_printf(cli_stdout, "Wall ahead!\n");
navigation_status.current_state = AVOID_WALL;
}
}
else if (navigation_status.current_state == AVOID_WALL){
//We check if we're not facing the wall anymore
if ((left_s < BACKWARD_THRESHOLD/2) || (right_s < BACKWARD_THRESHOLD/2)){
navigation_status.current_state = SPACE_NEEDED;
}
if (!((front1_s < BACKWARD_THRESHOLD) && (front2_s < BACKWARD_THRESHOLD))){
if (smallest > navigation_status.max_dist_obs){
navigation_status.current_state = GO_TO_TARGET;
GPIO_write(Board_OBS_A_EN, 0);
}else{
navigation_status.current_state = AVOID_OBSTACLE;
}
}
}
else if (navigation_status.current_state == SPACE_NEEDED){
if (!((left_s < BACKWARD_THRESHOLD/2) || (right_s < BACKWARD_THRESHOLD/2))){
//Check if we face another type of obstacle or if path is free
if (smallest < navigation_status.max_dist_obs){
//We use a different strategy depending on the type of obstacle
if (front1_s < BACKWARD_THRESHOLD && front2_s < BACKWARD_THRESHOLD){
serial_printf(cli_stdout, "Wall ahead!\n");
navigation_status.current_state = AVOID_WALL;
}else
navigation_status.current_state = AVOID_OBSTACLE;
}else{
navigation_status.current_state = GO_TO_TARGET;
GPIO_write(Board_OBS_A_EN, 0);
}
}
}
else
{
//normal operation: continue on mission items
if(mission_items.item[mission_items.current_index].current)
{
// Only go to target if mb is armed and rover must go to home OR sbc is armed and ready to record
#ifdef CONTINUE_WAYPOINTS_IMMEDIATELY
if(comm_mainboard_armed() == 1){
#else
if(comm_mainboard_armed() == 1 && (mission_items.current_index == 0 || comm_sbc_armed() == 1)){
#endif
navigation_status.current_state = GO_TO_TARGET;
}
else
{
navigation_status.current_state = STOP;
}
}
if(navigation_status.current_state == GO_TO_TARGET)
{
ultrasonic_get_distance(distance_values);
if(navigation_status.distance_to_target < TARGET_REACHED_DISTANCE)
{
navigation_mission_item_reached();
}
else if (smallest < navigation_status.max_dist_obs)
{
GPIO_write(Board_OBS_A_EN, 1);
//We use a different strategy depending on the type of obstacle
if (front1_s < BACKWARD_THRESHOLD && front2_s < BACKWARD_THRESHOLD)
{
serial_printf(cli_stdout, "Wall ahead!\n");
navigation_status.current_state = AVOID_WALL;
}
else if ((left_s < BACKWARD_THRESHOLD/2) || (right_s < BACKWARD_THRESHOLD/2))
{
navigation_status.current_state = SPACE_NEEDED;
}
else
navigation_status.current_state = AVOID_OBSTACLE;
}
}
}
}
#if defined (NAVIGATION_TEST)
void navigation_move();
#else
void navigation_move()
{
static int32_t lspeed, rspeed;
double angular = 0;
//only move if not on halt AND state = go_to_target
if(navigation_status.halt == 0)
{
if(navigation_status.current_state == GO_TO_TARGET)
{
angular = pid_update(&pid_a, navigation_status.angle_to_target) * PID_SCALING_FACTOR;
if (angular > 0){
lspeed = PWM_SPEED_100;
rspeed = PWM_SPEED_100 - (int32_t)(angular);
if (rspeed < PWM_MIN_CURVE_SPEED)
rspeed = PWM_MIN_CURVE_SPEED;
}else if (angular <= 0){
rspeed = PWM_SPEED_100;
lspeed = PWM_SPEED_100 + (int32_t)(angular);
if (lspeed < PWM_MIN_CURVE_SPEED)
lspeed = PWM_MIN_CURVE_SPEED;
}
motors_wheels_move((int32_t)lspeed, (int32_t)rspeed, (int32_t)lspeed, (int32_t)rspeed);
navigation_status.motor_values[0] = lspeed; //backup
navigation_status.motor_values[1] = rspeed;
}
else if(navigation_status.current_state == STOP)
{
motors_wheels_stop();
navigation_status.motor_values[0] = 0;
navigation_status.motor_values[1] = 0;
}
// else if(navigation_status.current_state == AVOID_OBSTACLE)
// {
// int32_t distance_values[N_ULTRASONIC_SENSORS];
// ultrasonic_get_distance(distance_values);
// ultrasonic_check_distance(distance_values, navigation_status.motor_values, PWM_SPEED_80); //80% of speed to move slower than in normal mode
//
// motors_wheels_move(navigation_status.motor_values[0], navigation_status.motor_values[1],
// navigation_status.motor_values[0], navigation_status.motor_values[1]);
// }else if (navigation_status.current_state == AVOID_WALL){
// // move backwards in opposite curving direction (TODO: to be tested)
// lspeed = -navigation_status.motor_values[1];
// rspeed = -navigation_status.motor_values[0];
// motors_wheels_move((int32_t)lspeed, (int32_t)rspeed, (int32_t)lspeed, (int32_t)rspeed);
// }else if (navigation_status.current_state == SPACE_NEEDED){
// navigation_status.motor_values[0] = -PWM_SPEED_100;
// navigation_status.motor_values[1] = -PWM_SPEED_100;
// motors_wheels_move(navigation_status.motor_values[0], navigation_status.motor_values[1],
// navigation_status.motor_values[0], navigation_status.motor_values[1]);
// }
}
else
{
navigation_status.motor_values[0] = 0;
navigation_status.motor_values[1] = 0;
motors_wheels_stop();
}
// serial_printf(cli_stdout, "speed l=%d, r=%d \n", motor_values[0], motor_values[1]);
}
#endif
void navigation_change_gain(char pid, char type, float gain)
{
if (pid == 'a'){
if(type == 'p')
pid_a.pGain = gain;
else if (type == 'i')
pid_a.iGain = gain;
else if (type == 'd')
pid_a.dGain = gain;
}
}
#if defined (NAVIGATION_TEST)
void navigation_init();
#else
void navigation_init()
{
cli_init();
motors_init();
ultrasonic_init();
pid_init(&pid_a, PGAIN_A, IGAIN_A, DGAIN_A, MOTOR_IMAX, MOTOR_IMIN);
navigation_status.lat_rover = INIT_LAT;
navigation_status.lon_rover = INIT_LON;
navigation_status.heading_rover = 0.0;
navigation_status.lat_target = TARGET_LAT;
navigation_status.lon_target = TARGET_LON;
navigation_status.distance_to_target = 0.0;
navigation_status.angle_to_target = 0.0;
navigation_status.current_state = STOP;
navigation_status.max_dist_obs = OBSTACLE_MAX_DIST;
navigation_status.halt = 0;
navigation_status.position_valid = false;
GPIO_write(Board_OBS_A_EN, 1);
mission_items.count = 0;
}
int main(void)
{
//Calibração do sensor
/*configPWM();
setServoAngle(0);
while(1){
}*/
double dt = 0.0;
double curr_err = 0;
float PID_Input = 0;
float PID_Output = 0;
int i_PID_Input = 0;
int i_Output = 0;
char c_PID_Input[4];
char c_Output[4];
char c_adc_value[4];
float gain = -20.0;
ADC_config();
ADC_Start();
USART_config();
configPWM();
setServoAngle(0);
ConfigSerie_BT();
pid_start();
pid_timer_config();
configMotorPWM(500);
setMotorDirection(1);
duty_cycle = 10;
while(1)
{
if(mode == 1)
{
if(all_read == 1)
{
PID_Input = Centroid_Algorithm();
curr_err = (3.5 - PID_Input);
dt = TCNT5;
dt = (dt*16)/1000000;
pid_update(ptr, curr_err,dt);
PID_Output = PID.control;
itoa(i_Output, c_Output, 10);
SendString((char*)"\n-------------------\n");
SendString((char*)"\nSaída_PID 1: ");
SendString(c_Output);
PID_Output = PID_Output*gain;
if(PID_Output > 75)
PID_Output = 75;
if(PID_Output < -75)
PID_Output = -75;
setServoAngle(PID_Output);
i_PID_Input = (int) PID_Input;
i_Output = (int) PID_Output;
itoa(i_PID_Input, c_PID_Input ,10);
itoa(i_Output, c_Output, 10);
SendString((char*)"\nLeituras: ");
for(int i = 0; i < 8; i++)
{
itoa(adc_value[i], c_adc_value, 10);
SendString(c_adc_value);
UsartSendByte(' ');
}
SendString((char*)"\nEntrada PID: ");
SendString(c_PID_Input);
SendString((char*)"\nSaída_PID 2: ");
SendString(c_Output);
all_read = 0;
ADMUX &= 0XE0; //Limpa ADMUX
ADMUX |= channel; //Selecciona o canal 0
ADCSRA |= (1<<ADSC); //Inicia conversão
}
}
if(mode == 0)
{
//
}
}
}
void core_turn_coordinator_update(core_context *context, void *setting, float dt) {
core_turn_coordinator *coordinator = (core_turn_coordinator *)setting;
// coordinate the turn by zeroing lateral G's
pid_update(&coordinator->rudderController, context->sensor_state.aB, dt);
context->effector_state.rud = coordinator->rudderController.output;
}
void neural_task(void *p)
{
while(1){
detachInterrupt(EXTI_Line0); /*close external interrupt 0*/
detachInterrupt(EXTI_Line1); /*close external interrupt 1*/
detachInterrupt(EXTI_Line2); /*close external interrupt 2*/
detachInterrupt(EXTI_Line3); /*close external interrupt 3*/
if(car_state == CAR_STATE_MOVE_FORWARD){
getMotorData();
rin = move_speed;
neural_update(&n_r, rin, speed_right_counter_1, distance_right_counter);
neural_update(&n_l, rin, speed_left_counter_1, distance_left_counter);
data_record();
float input_l = n_l.u;
float input_r = n_r.u;
proc_cmd("forward", base_pwm_l+(int)input_l, base_pwm_r+(int)input_r);
}
else if (car_state == CAR_STATE_MOVE_BACK)
{
getMotorData();
float err = 0;
rin = 10;
neural_update(&n_r_back, rin, speed_right_counter_1, distance_right_counter);
neural_update(&n_l_back, rin, speed_left_counter_1, distance_left_counter);
float input_l = n_l_back.u_1;
float input_r = n_r_back.u_1;
proc_cmd("forward", base_pwm_l - input_l, base_pwm_r - input_r);
}
else if (car_state == CAR_STATE_MOVE_LEFT){
rin = 5;
getMotorData();
if (distance_right_counter > MOVE_LEFT_RIGHT_PERIOD)
{
rin = 0;
pid_update(&n_r, rin, speed_right_counter_1, distance_right_counter);
}else{
pid_update(&n_r, rin, speed_right_counter_1, distance_right_counter);
}
if (distance_left_counter > MOVE_LEFT_RIGHT_PERIOD)
{
rin = 0;
pid_update(&n_l_back, rin, speed_left_counter_1, distance_left_counter);
}else{
pid_update(&n_l_back, rin, speed_left_counter_1, distance_left_counter);
}
float input_l = n_l_back.u;
float input_r = n_r.u;
proc_cmd("left", base_pwm_l-(int)input_l, base_pwm_r+(int)input_r);
}
else if (car_state == CAR_STATE_MOVE_RIGHT){
rin = 5;
getMotorData();
if (distance_right_counter > MOVE_LEFT_RIGHT_PERIOD)
{
rin = 0;
pid_update(&n_r_back, rin, speed_right_counter_1, distance_right_counter);
}else{
pid_update(&n_r_back, rin, speed_right_counter_1, distance_right_counter);
}
if (distance_left_counter > MOVE_LEFT_RIGHT_PERIOD)
{
rin = 0;
pid_update(&n_l, rin, speed_left_counter_1, distance_left_counter);
}else{
pid_update(&n_l, rin, speed_left_counter_1, distance_left_counter);
}
float input_l = n_l.u;
float input_r = n_r_back.u;
proc_cmd("right", base_pwm_l+(int)input_l, base_pwm_r-(int)input_r);
}
else if(car_state == CAR_STATE_STOPPING){
getMotorData();
if (last_state == CAR_STATE_MOVE_FORWARD)
{
if ((speed_left_counter_1 > 2) && (speed_right_counter_1 > 2))
{
rin = 0;
pid_update(&n_l, rin, speed_left_counter_1, distance_left_counter);
pid_update(&n_r, rin, speed_right_counter_1, distance_right_counter);
float input_l = n_l.u;
float input_r = n_r.u;
proc_cmd("forward", base_pwm_l + input_l, base_pwm_r + input_r);
}
else{
record_controller_base(&n_r);
record_controller_base(&n_l);
controller_reset(&n_r);
controller_reset(&n_l);
car_state = CAR_STATE_IDLE;
proc_cmd("forward", base_pwm_l, base_pwm_r);
data_sending = 1;
}
}
else if (last_state == CAR_STATE_MOVE_BACK)
{
if ((speed_left_counter_1 > 2) && (speed_right_counter_1 > 2))
{
rin = 0;
pid_update(&n_l_back, rin, speed_left_counter_1, distance_left_counter);
pid_update(&n_r_back, rin, speed_right_counter_1, distance_right_counter);
float input_l = n_l_back.u;
float input_r = n_r_back.u;
proc_cmd("forward", base_pwm_l - input_l, base_pwm_r - input_r);
}
else{
record_controller_base(&n_r_back);
record_controller_base(&n_l_back);
controller_reset(&n_r_back);
controller_reset(&n_l_back);
car_state = CAR_STATE_IDLE;
proc_cmd("forward", base_pwm_l, base_pwm_r);
data_sending = 1;
}
}
else if (last_state == CAR_STATE_MOVE_LEFT)
{
if ((speed_left_counter_1 > 2) && (speed_right_counter_1 > 2))
{
rin = 0;
pid_update(&n_l_back, rin, speed_left_counter_1, distance_left_counter);
pid_update(&n_r, rin, speed_right_counter_1, distance_right_counter);
float input_l = n_l_back.u;
float input_r = n_r.u;
proc_cmd("forward", base_pwm_l - input_l, base_pwm_r + input_r);
}
else{
//record_controller_base(&n_r);
//record_controller_base(&n_l_back);
controller_reset(&n_r);
controller_reset(&n_l_back);
car_state = CAR_STATE_IDLE;
proc_cmd("forward", base_pwm_l, base_pwm_r);
data_sending = 1;
}
}
else if (last_state == CAR_STATE_MOVE_RIGHT)
{
if ((speed_left_counter_1 > 2) && (speed_right_counter_1 > 2))
{
rin = 0;
pid_update(&n_l, rin, speed_left_counter_1, distance_left_counter);
pid_update(&n_r_back, rin, speed_right_counter_1, distance_right_counter);
float input_l = n_l.u;
float input_r = n_r_back.u;
proc_cmd("forward", base_pwm_l + input_l, base_pwm_r - input_r);
}
else{
//record_controller_base(&n_r_back);
//record_controller_base(&n_l);
controller_reset(&n_r_back);
controller_reset(&n_l);
car_state = CAR_STATE_IDLE;
proc_cmd("forward", base_pwm_l, base_pwm_r);
data_sending = 1;
}
}
}
else if(car_state == CAR_STATE_IDLE){
proc_cmd("forward", base_pwm_l, base_pwm_r);
speeed_initialize();
distance_initialize();
}
else{
}
attachInterrupt(EXTI_Line0);
attachInterrupt(EXTI_Line1);
attachInterrupt(EXTI_Line2);
attachInterrupt(EXTI_Line3);
vTaskDelay(NEURAL_PERIOD);
}
}
int rp_set_params(rp_app_params_t *p, int len)
{
int i;
int fpga_update = 1;
int params_change = 0;
int awg_params_change = 0;
int pid_params_change = 0;
TRACE("%s()\n", __FUNCTION__);
if(len > PARAMS_NUM) {
fprintf(stderr, "Too many parameters, max=%d\n", PARAMS_NUM);
return -1;
}
pthread_mutex_lock(&rp_main_params_mutex);
for(i = 0; i < len || p[i].name != NULL; i++) {
int p_idx = -1;
int j = 0;
/* Search for correct parameter name in defined parameters */
while(rp_main_params[j].name != NULL) {
int p_strlen = strlen(p[i].name);
if(p_strlen != strlen(rp_main_params[j].name)) {
j++;
continue;
}
if(!strncmp(p[i].name, rp_main_params[j].name, p_strlen)) {
p_idx = j;
break;
}
j++;
}
if(p_idx == -1) {
fprintf(stderr, "Parameter %s not found, ignoring it\n", p[i].name);
continue;
}
if(rp_main_params[p_idx].read_only)
continue;
if(rp_main_params[p_idx].value != p[i].value) {
if(p_idx < PARAMS_AWG_PARAMS)
params_change = 1;
if ( (p_idx >= PARAMS_AWG_PARAMS) && (p_idx < PARAMS_PID_PARAMS) )
awg_params_change = 1;
if(p_idx >= PARAMS_PID_PARAMS)
pid_params_change = 1;
if(rp_main_params[p_idx].fpga_update)
fpga_update = 1;
}
if(rp_main_params[p_idx].min_val > p[i].value) {
fprintf(stderr, "Incorrect parameters value: %f (min:%f), "
" correcting it\n", p[i].value, rp_main_params[p_idx].min_val);
p[i].value = rp_main_params[p_idx].min_val;
} else if(rp_main_params[p_idx].max_val < p[i].value) {
fprintf(stderr, "Incorrect parameters value: %f (max:%f), "
" correcting it\n", p[i].value, rp_main_params[p_idx].max_val);
p[i].value = rp_main_params[p_idx].max_val;
}
rp_main_params[p_idx].value = p[i].value;
}
transform_from_iface_units(&rp_main_params[0]);
pthread_mutex_unlock(&rp_main_params_mutex);
/* Set parameters in HW/FPGA only if they have changed */
if(params_change || (params_init == 0)) {
pthread_mutex_lock(&rp_main_params_mutex);
/* Xmin & Xmax public copy to be served to clients */
rp_main_params[GUI_XMIN].value = p[MIN_GUI_PARAM].value;
rp_main_params[GUI_XMAX].value = p[MAX_GUI_PARAM].value;
transform_acq_params(rp_main_params);
pthread_mutex_unlock(&rp_main_params_mutex);
/* First do health check and then send it to the worker! */
int mode = rp_main_params[TRIG_MODE_PARAM].value;
int time_range = rp_main_params[TIME_RANGE_PARAM].value;
int time_unit = 2;
/* Get info from FPGA module about clocks/decimation, ...*/
int dec_factor = osc_fpga_cnv_time_range_to_dec(time_range);
float smpl_period = c_osc_fpga_smpl_period * dec_factor;
/* t_delay - trigger delay in seconds */
float t_delay = rp_main_params[TRIG_DLY_PARAM].value;
float t_unit_factor = 1; /* to convert to seconds */
/* Our time window with current settings:
* - time_delay is added later, when we check if it is correct
* setting
*/
float t_min = 0;
float t_max = ((OSC_FPGA_SIG_LEN-1) * smpl_period);
float t_max_minus = ((OSC_FPGA_SIG_LEN-6) * smpl_period);
params_init = 1;
/* in time units time_unit, needs to be converted */
float t_start = rp_main_params[MIN_GUI_PARAM].value;
float t_stop = rp_main_params[MAX_GUI_PARAM].value;
int t_start_idx;
int t_stop_idx;
int t_step_idx = 0;
/* If auto-set algorithm was requested do not set other parameters */
if(rp_main_params[AUTO_FLAG_PARAM].value == 1) {
auto_in_progress = 1;
forcex_state = 0;
rp_osc_clean_signals();
rp_osc_worker_change_state(rp_osc_auto_set_state);
/* AUTO_FLAG_PARAM is cleared when Auto-set algorithm finishes */
/* Wait for auto-set algorithm to finish or timeout */
int timeout = 10000000; // [us]
const int step = 50000; // [us]
rp_osc_worker_state_t state;
while (timeout > 0) {
rp_osc_worker_get_state(&state);
if (state != rp_osc_auto_set_state) {
break;
}
usleep(step);
timeout -= step;
}
if (timeout <= 0) {
fprintf(stderr, "AUTO: Timeout waiting for AUTO-set algorithm to finish.\n");
}
auto_in_progress = 0;
return 0;
}
/* If AUTO trigger mode, reset trigger delay */
if(mode == 0)
t_delay = 0;
if(dec_factor < 0) {
fprintf(stderr, "Incorrect time range: %d\n", time_range);
return -1;
}
/* Pick time unit and unit factor corresponding to current time range. */
if((time_range == 0) || (time_range == 1)) {
time_unit = 0;
t_unit_factor = 1e6;
} else if((time_range == 2) || (time_range == 3)) {
time_unit = 1;
t_unit_factor = 1e3;
}
rp_main_params[TIME_UNIT_PARAM].value = time_unit;
TRACE("PC: time_(R,U) = (%d, %d)\n", time_range, time_unit);
/* Check if trigger delay in correct range, otherwise correct it
* Correct trigger delay is:
* t_delay >= -t_max_minus
* t_delay <= OSC_FPGA_MAX_TRIG_DELAY
*/
if(t_delay < -t_max_minus) {
t_delay = -t_max_minus;
} else if(t_delay > (OSC_FPGA_TRIG_DLY_MASK * smpl_period)) {
t_delay = OSC_FPGA_TRIG_DLY_MASK * smpl_period;
} else {
t_delay = round(t_delay / smpl_period) * smpl_period;
}
t_min = t_min + t_delay;
t_max = t_max + t_delay;
rp_main_params[TRIG_DLY_PARAM].value = t_delay;
/* Convert to seconds */
t_start = t_start / t_unit_factor;
t_stop = t_stop / t_unit_factor;
TRACE("PC: t_stop = %.9f\n", t_stop);
/* Select correct time window with this settings:
* time window is defined from:
* ([ 0 - 16k ] * smpl_period) + trig_delay */
/* round to correct/possible values - convert to nearest index
* and back
*/
t_start_idx = round(t_start / smpl_period);
t_stop_idx = round(t_stop / smpl_period);
t_start = (t_start_idx * smpl_period);
t_stop = (t_stop_idx * smpl_period);
if(t_start < t_min)
t_start = t_min;
if(t_stop > t_max)
t_stop = t_max;
if(t_stop <= t_start )
t_stop = t_max;
/* Correct the window according to possible decimations - always
* provide at least the data demanded by the user (ceil() instead
* of round())
*/
t_start_idx = round(t_start / smpl_period);
t_stop_idx = round(t_stop / smpl_period);
if((((t_stop_idx-t_start_idx)/(float)(((int)rp_get_params_bode(5))-1))) >= 1) {
t_step_idx = ceil((t_stop_idx-t_start_idx)/(float)(((int)rp_get_params_bode(5))-1));
int max_step = OSC_FPGA_SIG_LEN/((int)rp_get_params_bode(5));
if(t_step_idx > max_step)
t_step_idx = max_step;
t_stop = t_start + ((int)rp_get_params_bode(5)) * t_step_idx * smpl_period;
}
TRACE("PC: t_stop (rounded) = %.9f\n", t_stop);
/* write back and convert to set units */
rp_main_params[MIN_GUI_PARAM].value = t_start;
rp_main_params[MAX_GUI_PARAM].value = t_stop;
rp_osc_worker_update_params((rp_app_params_t *)&rp_main_params[0],
fpga_update);
/* check if we need to change state */
switch(mode) {
case 0:
/* auto */
rp_osc_worker_change_state(rp_osc_auto_state);
break;
case 1:
/* normal */
rp_osc_worker_change_state(rp_osc_normal_state);
break;
case 2:
/* single - clear last ok buffer */
rp_osc_worker_change_state(rp_osc_idle_state);
rp_osc_clean_signals();
break;
default:
return -1;
}
if(rp_main_params[SINGLE_BUT_PARAM].value == 1) {
rp_main_params[SINGLE_BUT_PARAM].value = 0;
rp_osc_clean_signals();
rp_osc_worker_change_state(rp_osc_single_state);
}
}
if(awg_params_change) {
/* Correct frequencies if needed */
rp_main_params[GEN_SIG_FREQ_CH1].value =
rp_gen_limit_freq(rp_main_params[GEN_SIG_FREQ_CH1].value,
rp_main_params[GEN_SIG_TYPE_CH1].value);
rp_main_params[GEN_SIG_FREQ_CH2].value =
rp_gen_limit_freq(rp_main_params[GEN_SIG_FREQ_CH2].value,
rp_main_params[GEN_SIG_TYPE_CH2].value);
if(generate_update(&rp_main_params[0]) < 0) {
return -1;
}
}
if (pid_params_change) {
if(pid_update(&rp_main_params[0]) < 0) {
return -1;
}
}
return 0;
}
