Publishing for Biomimetics.
Dependencies: CRC MODDMA MODSERIAL MPU6050IMU PID QEI mbed-rtos mbed-src
Diff: control.h
- Revision:
- 0:8cfa73bb68e4
diff -r 000000000000 -r 8cfa73bb68e4 control.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/control.h Tue May 31 17:04:59 2016 +0000 @@ -0,0 +1,58 @@ +#ifndef CONTROL_H +#define CONTROL_H + +#include "mbed.h" + +#include "rtos.h" +#include "PID.h" + +#include "protocol.h" +#include "packet_parser.h" +#include "sensors.h" + +#define MOTOR_RIGHT 1 +#define MOTOR_LEFT 0 + +class Control { + + public: + + Control( + PinName left_mot_0_pin, PinName left_mot_1_pin, + PinName right_mot_0_pin, PinName right_mot_1_pin, + Sensors *sensors, uint32_t tick_per_rev, + float kP, float kI, float kD, float period, float velocity_max, float pid_dead_band + ); + + void set_setpoints(float left, float right); + + void fill_pid_packet(packet_t* pkt); + + void fill_sensor_packet(packet_t* pkt); + + private: + + Sensors* sensors_; + + RtosTimer control_timer_; + + PID* pids_[2]; + + void pid_init(int motor, float kP, float kI, float kD, float period, float velocity_max); + + void set_motor_pwm(int motor, float value); + + static void control_helper(const void* p); + void control_update(void); + + PwmOut* motors_[2][2]; + + float tick_to_angular_velocity_; + float pid_dead_band_; + + float last_positions_[2]; + float velocities_[2]; + float pwms_[2]; +}; + +#endif \ No newline at end of file