Drive thread library
drive.h@0:e5e9b52a35ae, 2017-08-23 (annotated)
- Committer:
- williampeers
- Date:
- Wed Aug 23 02:25:59 2017 +0000
- Revision:
- 0:e5e9b52a35ae
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
williampeers | 0:e5e9b52a35ae | 1 | #ifndef __DRIVE_INCLUDED__ |
williampeers | 0:e5e9b52a35ae | 2 | #define __DRIVE_INCLUDED__ |
williampeers | 0:e5e9b52a35ae | 3 | |
williampeers | 0:e5e9b52a35ae | 4 | #include "mbed.h" |
williampeers | 0:e5e9b52a35ae | 5 | #include "pid_controller.h" |
williampeers | 0:e5e9b52a35ae | 6 | |
williampeers | 0:e5e9b52a35ae | 7 | #define PWMPERIOD 4096 //PWM period in us |
williampeers | 0:e5e9b52a35ae | 8 | |
williampeers | 0:e5e9b52a35ae | 9 | #define KP 3 |
williampeers | 0:e5e9b52a35ae | 10 | #define KI 0.1 |
williampeers | 0:e5e9b52a35ae | 11 | #define KD 0.1 |
williampeers | 0:e5e9b52a35ae | 12 | #define PIDSAMPLERATE 0.1 |
williampeers | 0:e5e9b52a35ae | 13 | |
williampeers | 0:e5e9b52a35ae | 14 | class Driver { |
williampeers | 0:e5e9b52a35ae | 15 | private: |
williampeers | 0:e5e9b52a35ae | 16 | PIDControl pidR; |
williampeers | 0:e5e9b52a35ae | 17 | PIDControl pidL; |
williampeers | 0:e5e9b52a35ae | 18 | |
williampeers | 0:e5e9b52a35ae | 19 | InterruptIn l_encoder1; //Left motor encoder signal 1 |
williampeers | 0:e5e9b52a35ae | 20 | InterruptIn l_encoder2; //Left motor encoder signal 2 |
williampeers | 0:e5e9b52a35ae | 21 | InterruptIn r_encoder1; //Right motor encoder signal 3 |
williampeers | 0:e5e9b52a35ae | 22 | InterruptIn r_encoder2; //Right motor encoder signal 4 |
williampeers | 0:e5e9b52a35ae | 23 | |
williampeers | 0:e5e9b52a35ae | 24 | PwmOut left_motor; // Forward Motor direction |
williampeers | 0:e5e9b52a35ae | 25 | PwmOut left_motor_rev; // Reverse Motor direction |
williampeers | 0:e5e9b52a35ae | 26 | PwmOut right_motor; // Forward Motor direction |
williampeers | 0:e5e9b52a35ae | 27 | PwmOut right_motor_rev; // Reverse Motor direction |
williampeers | 0:e5e9b52a35ae | 28 | |
williampeers | 0:e5e9b52a35ae | 29 | long lSpeedGoal;//Goal motor speed |
williampeers | 0:e5e9b52a35ae | 30 | long rSpeedGoal; |
williampeers | 0:e5e9b52a35ae | 31 | |
williampeers | 0:e5e9b52a35ae | 32 | long l_RPM;//Measured current speed |
williampeers | 0:e5e9b52a35ae | 33 | long r_RPM; |
williampeers | 0:e5e9b52a35ae | 34 | |
williampeers | 0:e5e9b52a35ae | 35 | Mutex lock; |
williampeers | 0:e5e9b52a35ae | 36 | |
williampeers | 0:e5e9b52a35ae | 37 | Thread driver_thread; |
williampeers | 0:e5e9b52a35ae | 38 | |
williampeers | 0:e5e9b52a35ae | 39 | unsigned int LoutA; //Holds left output A of encoder signal |
williampeers | 0:e5e9b52a35ae | 40 | unsigned int LoutB; //Holds left output B of encoder signal |
williampeers | 0:e5e9b52a35ae | 41 | unsigned int RoutA; //Holds Right output A of encoder signal |
williampeers | 0:e5e9b52a35ae | 42 | unsigned int RoutB; //Holds Right output B of encoder signal |
williampeers | 0:e5e9b52a35ae | 43 | |
williampeers | 0:e5e9b52a35ae | 44 | unsigned char l_enc_val; //Sequence that holds current and previous encoder combination values |
williampeers | 0:e5e9b52a35ae | 45 | unsigned char r_enc_val; |
williampeers | 0:e5e9b52a35ae | 46 | char l_enc_direction; |
williampeers | 0:e5e9b52a35ae | 47 | char r_enc_direction; |
williampeers | 0:e5e9b52a35ae | 48 | volatile long l_enc_count; //Holds current direction of left motor |
williampeers | 0:e5e9b52a35ae | 49 | volatile long r_enc_count; //Holds current direction of right motor |
williampeers | 0:e5e9b52a35ae | 50 | |
williampeers | 0:e5e9b52a35ae | 51 | //Ticker timer; |
williampeers | 0:e5e9b52a35ae | 52 | volatile long l_old_enc_count; //Hold old encounter value for computation |
williampeers | 0:e5e9b52a35ae | 53 | volatile long r_old_enc_count; |
williampeers | 0:e5e9b52a35ae | 54 | |
williampeers | 0:e5e9b52a35ae | 55 | void l_encode_trig();//function for handling encoder trigger |
williampeers | 0:e5e9b52a35ae | 56 | void r_encode_trig(); |
williampeers | 0:e5e9b52a35ae | 57 | |
williampeers | 0:e5e9b52a35ae | 58 | void run();//function to run in thread (contains infinite loop) |
williampeers | 0:e5e9b52a35ae | 59 | |
williampeers | 0:e5e9b52a35ae | 60 | |
williampeers | 0:e5e9b52a35ae | 61 | DigitalOut my_led; |
williampeers | 0:e5e9b52a35ae | 62 | |
williampeers | 0:e5e9b52a35ae | 63 | public: |
williampeers | 0:e5e9b52a35ae | 64 | Driver(osPriority, int); |
williampeers | 0:e5e9b52a35ae | 65 | void start(); //starts thread "pid" running run() |
williampeers | 0:e5e9b52a35ae | 66 | //void setVelocity(int speed, char* directoin);//called to set desired speed |
williampeers | 0:e5e9b52a35ae | 67 | void setVelocity(int lSpeed, int rSpeed); |
williampeers | 0:e5e9b52a35ae | 68 | }; |
williampeers | 0:e5e9b52a35ae | 69 | |
williampeers | 0:e5e9b52a35ae | 70 | #endif |