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