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); } } }