Diff: control.h

Revision:

0:8cfa73bb68e4

--- /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