void wheel_update_pid (void)
{
pid_l.setpoint = spd_set_l; /* update PID controller (left) */
pid_l.measured = pid_ticks_l;
pid_ticks_l = 0;
pid_int_update (&pid_l);
if (pid_l.output >= 0)
prop_set_l += pid_l.output;
else
prop_set_l -= -pid_l.output;
if (prop_set_l > 255)
prop_set_l = 255;
else if (prop_set_l <-255)
prop_set_l = -255;
pid_r.setpoint = spd_set_r; /* update PID controller (right) */
pid_r.measured = pid_ticks_r;
pid_ticks_r = 0;
pid_int_update (&pid_r);
if (pid_r.output >= 0)
prop_set_r += pid_r.output;
else
prop_set_r -= -pid_r.output;
if (prop_set_r > 255)
prop_set_r = 255;
else if (prop_set_r <-255)
prop_set_r = -255;
motor_update();
}
/************************************************************
Main Loop
************************************************************/
int main(void)
{
/* Confirm Power */
m_red(ON);
/* Initializations */
initialize_robockey();
pause();
play();
/* Confirm successful initialization(s) */
m_green(ON);
/* Run */
while (1){
update_position();
if (check(ADCSRA,ADIF)){adc_update();} // Check if ADC conversion has completed
bot_behavior_update();
if (check(TIFR3,OCF3A)){motor_update();} // Check if timestep has completed
if (wifi_flag) {
wifi_flag = 0;
m_red(TOGGLE);
wireless_recieve();
}
}
}
void motor_reset() {
/* Begin with resetting axis Z if no reset is already in progress. It is
* imperative to first retract on axis Z separately from the others. Once
* that has been dealt with, axes X and Y may follow. */
if(bit_is_clear(motor_status, MTR_RESET)) {
motor_stop();
motor_status |= _BV(MTR_RESET) | _BV(MTR_IS_Z);
setup_axis(AXIS_Z, MTR_INC);
LOCK_DISABLE();
motor_start();
/* Axis Z has been reset. Now, axes X and Y may follow. */
} else if(bit_is_set(motor_status, MTR_RESET_Z_DONE)) {
/* Remove flag denoting axis Z is resetting. */
motor_status &= ~(_BV(MTR_IS_Z) | _BV(MTR_RESET_Z_DONE));
/* Reset axes X and Y. */
setup_axis(AXIS_Y, MTR_DEC);
setup_axis(AXIS_X, MTR_DEC);
LOCK_DISABLE(); /* Manually enable PWM propagation. */
motor_start();
/* All resetting stages have been completed. Reset #cur_pos and flags. */
} else if(bit_is_set(motor_status, MTR_RESET_X_DONE)
&& bit_is_set(motor_status, MTR_RESET_Y_DONE)) {
cur_pos.x = 0;
cur_pos.y = 0;
cur_pos.z = max_pos.z - 1;
motor_stop();
LOCK_ENABLE();
/* If this resetting cycle was initiated as a response to a limit being
* engaged while under normal motor operation, retry reaching #new_pos
* anew. */
if(bit_is_set(motor_status, MTR_LIMIT)) {
if(motor_update()) {
motor_stop();
MTR_CALL(cur_pos, MTR_EVT_OK);
}
} else {
new_pos = cur_pos;
MTR_CALL(cur_pos, MTR_EVT_OK);
}
motor_status &= ~(_BV(MTR_RESET) | _BV(MTR_LIMIT)
| _BV(MTR_RESET_X_DONE) | _BV(MTR_RESET_Y_DONE));
motor_status |= _BV(MTR_IS_RST_FRESH);
/* Reset is in progress. */
} else {
}
}
int8_t motor_set(Position target) {
/* Fail, if the motors are resetting or otherwise operated upon. */
if(bit_is_set(motor_status, MTR_RESET) || PWM_IS_ON()) return -1;
/* Fail, if target coordinates lay outside the available device space. */
if(target.x >= max_pos.x
|| target.y >= max_pos.y
|| target.z >= max_pos.z) {
return -1;
}
new_pos = target;
return motor_update();
}
//自稳模式
rt_err_t stable_mode(u8 var)
{
if (pwm.throttle > 0.05f && abs(ahrs.degree_pitch) < 40 && abs(ahrs.degree_roll) < 40)
{
yaw_exp += pwm.yaw * 0.5f;
if (yaw_exp > 360.0f)
yaw_exp -= 360.0f;
if (yaw_exp < 0.0f)
yaw_exp += 360.0f;
stable(pwm.pitch * 30.0f, pwm.roll * 30.0f, yaw_exp);
motor_update(pwm.throttle * 1000);
}
else
Motor_Set(60, 60, 60, 60);
return RT_EOK;
}
void wheel_update_open_loop (void)
{
prop_set_l = spd_set_l;
prop_set_r = spd_set_r;
motor_update();
}
int main()
{
int32_t bemf_vals[4] = {0,0,0,0};
int32_t bemf_vals_filt[4] = {0,0,0,0};
init();
// set up DMA/SPI buffers
init_rx_buffer();
init_tx_buffer();
// set up pid structs
pid_struct pid_structs[4];
{ uint8_t i;
for (i = 0; i < 4; ++i) init_pid_struct(&(pid_structs[i]), i);
}
update_dig_pin_configs();
config_adc_in_from_regs();
//debug_printf("starting\n");
int low_volt_alarmed = 0;
// Loop until button is pressed
uint32_t count = 0;
while (1)
{
count += 1;
{
// only sample motor backemf 1/4 of the time
const uint8_t bemf_update_time = (count % 4 == 1);
if (bemf_update_time)
{
// idle breaking
MOT0_DIR1_PORT->BSRRH |= MOT0_DIR1;
MOT0_DIR2_PORT->BSRRH |= MOT0_DIR2;
MOT1_DIR1_PORT->BSRRH |= MOT1_DIR1;
MOT1_DIR2_PORT->BSRRH |= MOT1_DIR2;
MOT2_DIR1_PORT->BSRRH |= MOT2_DIR1;
MOT2_DIR2_PORT->BSRRH |= MOT2_DIR2;
MOT3_DIR1_PORT->BSRRH |= MOT3_DIR1;
MOT3_DIR2_PORT->BSRRH |= MOT3_DIR2;
}
// let the motor coast
// delay_us(700);
// now I squeeze in most sensor updates instead of just sleeping
uint32_t before = usCount;
update_dig_pins();
int16_t batt = adc_update();
readAccel();
readMag();
readGyro();
if (batt < 636) // about 5.75 volts
{
configDigitalOutPin(LED1_PIN, LED1_PORT);
low_volt_alarmed = 1;
if (count % 50 < 10) // low duty cycle to save battery
{
LED1_PORT->BSRRL |= LED1_PIN; // ON
}
else
{
LED1_PORT->BSRRH |= LED1_PIN; // OFF
}
}
else if (low_volt_alarmed)
{
// make sure led is off coming out of the low voltage alarm
configDigitalOutPin(LED1_PIN, LED1_PORT);
LED1_PORT->BSRRH |= LED1_PIN; // OFF
low_volt_alarmed = 0;
}
if (adc_dirty)
{
adc_dirty = 0;
config_adc_in_from_regs();
}
if (dig_dirty)
{
dig_dirty = 0;
update_dig_pin_configs();
}
uint32_t sensor_update_time = usCount - before;
const uint16_t us_delay_needed = 700;
const uint8_t got_time_to_burn = sensor_update_time < us_delay_needed;
if (got_time_to_burn)
{
// sleep the remainder of the coasting period before sampling bemf
delay_us(us_delay_needed-sensor_update_time);
}
if (bemf_update_time)
{
update_bemfs(bemf_vals, bemf_vals_filt);
// set the motor directions back up
update_motor_modes();
uint8_t channel;
for (channel = 0; channel < 4; ++channel)
{
uint8_t shift = 2*channel;
uint8_t motor_mode = (aTxBuffer[REG_RW_MOT_MODES] & (0b11 << shift)) >> shift;
motor_update(bemf_vals[channel], bemf_vals_filt[channel], &pid_structs[channel], channel, motor_mode);
}
}
else
{
// updating sensors doesnt' take as long as all of the adc for backemf updates
// sleep a bit so that our time through the loop is consistent for PID control
delay_us(222);
}
}
}
