Austin Buchan / mbed_zumy_packet

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?

UserRevisionLine numberNew 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 }