Publishing for Biomimetics.
Dependencies: CRC MODDMA MODSERIAL MPU6050IMU PID QEI mbed-rtos mbed-src
control.cpp@0:8cfa73bb68e4, 2016-05-31 (annotated)
- Committer:
- abuchan
- Date:
- Tue May 31 17:04:59 2016 +0000
- Revision:
- 0:8cfa73bb68e4
Publishing for Biomimetics.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
abuchan | 0:8cfa73bb68e4 | 1 | #include "control.h" |
abuchan | 0:8cfa73bb68e4 | 2 | |
abuchan | 0:8cfa73bb68e4 | 3 | Control::Control( |
abuchan | 0:8cfa73bb68e4 | 4 | PinName left_mot_0_pin, PinName left_mot_1_pin, |
abuchan | 0:8cfa73bb68e4 | 5 | PinName right_mot_0_pin, PinName right_mot_1_pin, |
abuchan | 0:8cfa73bb68e4 | 6 | Sensors *sensors, uint32_t tick_per_rev, |
abuchan | 0:8cfa73bb68e4 | 7 | float kP, float kI, float kD, float period, float velocity_max, |
abuchan | 0:8cfa73bb68e4 | 8 | float pid_dead_band |
abuchan | 0:8cfa73bb68e4 | 9 | ) : |
abuchan | 0:8cfa73bb68e4 | 10 | sensors_(sensors), |
abuchan | 0:8cfa73bb68e4 | 11 | control_timer_(&Control::control_helper,osTimerPeriodic,this), |
abuchan | 0:8cfa73bb68e4 | 12 | pid_dead_band_(pid_dead_band) |
abuchan | 0:8cfa73bb68e4 | 13 | { |
abuchan | 0:8cfa73bb68e4 | 14 | |
abuchan | 0:8cfa73bb68e4 | 15 | motors_[MOTOR_LEFT][0] = new PwmOut(left_mot_0_pin); |
abuchan | 0:8cfa73bb68e4 | 16 | motors_[MOTOR_LEFT][1] = new PwmOut(left_mot_1_pin); |
abuchan | 0:8cfa73bb68e4 | 17 | motors_[MOTOR_RIGHT][0] = new PwmOut(right_mot_0_pin); |
abuchan | 0:8cfa73bb68e4 | 18 | motors_[MOTOR_RIGHT][1] = new PwmOut(right_mot_1_pin); |
abuchan | 0:8cfa73bb68e4 | 19 | |
abuchan | 0:8cfa73bb68e4 | 20 | // For the LPC1768, all PWM channels are on the same timer, so setting one |
abuchan | 0:8cfa73bb68e4 | 21 | // period sets them all |
abuchan | 0:8cfa73bb68e4 | 22 | motors_[MOTOR_LEFT][0]->period_us(50); |
abuchan | 0:8cfa73bb68e4 | 23 | |
abuchan | 0:8cfa73bb68e4 | 24 | tick_to_angular_velocity_ = 2.0 * 3.14159265358979323846 / (float)(tick_per_rev * period); |
abuchan | 0:8cfa73bb68e4 | 25 | |
abuchan | 0:8cfa73bb68e4 | 26 | pid_init(MOTOR_LEFT,kP,kI,kD,period,velocity_max); |
abuchan | 0:8cfa73bb68e4 | 27 | pid_init(MOTOR_RIGHT,kP,kI,kD,period,velocity_max); |
abuchan | 0:8cfa73bb68e4 | 28 | |
abuchan | 0:8cfa73bb68e4 | 29 | control_timer_.start(period*1000); |
abuchan | 0:8cfa73bb68e4 | 30 | |
abuchan | 0:8cfa73bb68e4 | 31 | last_positions_[MOTOR_LEFT] = 0; |
abuchan | 0:8cfa73bb68e4 | 32 | last_positions_[MOTOR_RIGHT] = 0; |
abuchan | 0:8cfa73bb68e4 | 33 | } |
abuchan | 0:8cfa73bb68e4 | 34 | |
abuchan | 0:8cfa73bb68e4 | 35 | void Control::set_setpoints(float left, float right) { |
abuchan | 0:8cfa73bb68e4 | 36 | pids_[MOTOR_LEFT]->setSetPoint(left); |
abuchan | 0:8cfa73bb68e4 | 37 | pids_[MOTOR_RIGHT]->setSetPoint(right); |
abuchan | 0:8cfa73bb68e4 | 38 | } |
abuchan | 0:8cfa73bb68e4 | 39 | |
abuchan | 0:8cfa73bb68e4 | 40 | void Control::fill_pid_packet(packet_t* pkt) { |
abuchan | 0:8cfa73bb68e4 | 41 | pkt->header.type = PKT_TYPE_PID; |
abuchan | 0:8cfa73bb68e4 | 42 | pkt->header.length = sizeof(header_t) + sizeof(pid_data_t) + 1; |
abuchan | 0:8cfa73bb68e4 | 43 | pid_data_t* pid_data = (pid_data_t*)pkt->data_crc; |
abuchan | 0:8cfa73bb68e4 | 44 | pid_data->vel[MOTOR_LEFT] = velocities_[MOTOR_LEFT]; |
abuchan | 0:8cfa73bb68e4 | 45 | pid_data->vel[MOTOR_RIGHT] = velocities_[MOTOR_RIGHT]; |
abuchan | 0:8cfa73bb68e4 | 46 | pid_data->pwm[MOTOR_LEFT] = pwms_[MOTOR_LEFT]; |
abuchan | 0:8cfa73bb68e4 | 47 | pid_data->pwm[MOTOR_RIGHT] = pwms_[MOTOR_RIGHT]; |
abuchan | 0:8cfa73bb68e4 | 48 | } |
abuchan | 0:8cfa73bb68e4 | 49 | |
abuchan | 0:8cfa73bb68e4 | 50 | void Control::fill_sensor_packet(packet_t* pkt) { |
abuchan | 0:8cfa73bb68e4 | 51 | pkt->header.type = PKT_TYPE_SENSOR; |
abuchan | 0:8cfa73bb68e4 | 52 | pkt->header.length = sizeof(header_t) + sizeof(sensor_data_t) + 1; |
abuchan | 0:8cfa73bb68e4 | 53 | sensor_data_t* sensor_data = (sensor_data_t*)pkt->data_crc; |
abuchan | 0:8cfa73bb68e4 | 54 | sensor_data->velocity[MOTOR_LEFT] = velocities_[MOTOR_LEFT]; |
abuchan | 0:8cfa73bb68e4 | 55 | sensor_data->velocity[MOTOR_RIGHT] = velocities_[MOTOR_RIGHT]; |
abuchan | 0:8cfa73bb68e4 | 56 | } |
abuchan | 0:8cfa73bb68e4 | 57 | |
abuchan | 0:8cfa73bb68e4 | 58 | void Control::set_motor_pwm(int motor, float value) { |
abuchan | 0:8cfa73bb68e4 | 59 | if (value >= 0.0) { |
abuchan | 0:8cfa73bb68e4 | 60 | motors_[motor][0]->write(value); |
abuchan | 0:8cfa73bb68e4 | 61 | motors_[motor][1]->write(0.0); |
abuchan | 0:8cfa73bb68e4 | 62 | } else { |
abuchan | 0:8cfa73bb68e4 | 63 | motors_[motor][0]->write(0.0); |
abuchan | 0:8cfa73bb68e4 | 64 | motors_[motor][1]->write(-value); |
abuchan | 0:8cfa73bb68e4 | 65 | } |
abuchan | 0:8cfa73bb68e4 | 66 | } |
abuchan | 0:8cfa73bb68e4 | 67 | |
abuchan | 0:8cfa73bb68e4 | 68 | void Control::pid_init(int motor, float kP, float kI, float kD, float period, float velocity_max) { |
abuchan | 0:8cfa73bb68e4 | 69 | pids_[motor] = new PID(kP,kI,kD,period); |
abuchan | 0:8cfa73bb68e4 | 70 | pids_[motor]->setInputLimits(-velocity_max, velocity_max); |
abuchan | 0:8cfa73bb68e4 | 71 | pids_[motor]->setOutputLimits(-1.0,1.0); |
abuchan | 0:8cfa73bb68e4 | 72 | pids_[motor]->setBias(0.0); |
abuchan | 0:8cfa73bb68e4 | 73 | pids_[motor]->setSetPoint(0.0); |
abuchan | 0:8cfa73bb68e4 | 74 | } |
abuchan | 0:8cfa73bb68e4 | 75 | |
abuchan | 0:8cfa73bb68e4 | 76 | void Control::control_helper(const void* p) { |
abuchan | 0:8cfa73bb68e4 | 77 | Control* instance = (Control*)p; |
abuchan | 0:8cfa73bb68e4 | 78 | instance->control_update(); |
abuchan | 0:8cfa73bb68e4 | 79 | } |
abuchan | 0:8cfa73bb68e4 | 80 | |
abuchan | 0:8cfa73bb68e4 | 81 | void Control::control_update(void) { |
abuchan | 0:8cfa73bb68e4 | 82 | float positions[2]; |
abuchan | 0:8cfa73bb68e4 | 83 | |
abuchan | 0:8cfa73bb68e4 | 84 | sensors_->get_angles(positions); |
abuchan | 0:8cfa73bb68e4 | 85 | |
abuchan | 0:8cfa73bb68e4 | 86 | for (uint32_t i=0; i<2; i++) { |
abuchan | 0:8cfa73bb68e4 | 87 | velocities_[i] = tick_to_angular_velocity_ * (positions[i] - last_positions_[i]); |
abuchan | 0:8cfa73bb68e4 | 88 | last_positions_[i] = positions[i]; |
abuchan | 0:8cfa73bb68e4 | 89 | pids_[i]->setProcessValue(velocities_[i]); |
abuchan | 0:8cfa73bb68e4 | 90 | pwms_[i] = pids_[i]->compute(); |
abuchan | 0:8cfa73bb68e4 | 91 | if (fabs(pwms_[i]) < pid_dead_band_) |
abuchan | 0:8cfa73bb68e4 | 92 | pwms_[i] = 0.0; |
abuchan | 0:8cfa73bb68e4 | 93 | } |
abuchan | 0:8cfa73bb68e4 | 94 | |
abuchan | 0:8cfa73bb68e4 | 95 | set_motor_pwm(MOTOR_LEFT, pwms_[MOTOR_LEFT]); |
abuchan | 0:8cfa73bb68e4 | 96 | set_motor_pwm(MOTOR_RIGHT, pwms_[MOTOR_RIGHT]); |
abuchan | 0:8cfa73bb68e4 | 97 | } |