Wheel control software for satellite microcontroller running the motors on the Karbor
Dependencies: mbed ros_lib_melodic
src/MotorControl.h@6:ed47deb76adf, 2021-06-08 (annotated)
- Committer:
- krogedal
- Date:
- Tue Jun 08 20:55:08 2021 +0000
- Revision:
- 6:ed47deb76adf
- Parent:
- 5:44b2454a5eea
Might work now?
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
krogedal | 6:ed47deb76adf | 1 | /** |
krogedal | 6:ed47deb76adf | 2 | * @file MotorControl.h |
krogedal | 6:ed47deb76adf | 3 | * |
krogedal | 6:ed47deb76adf | 4 | * Class to control a motor with an encoder, inherits from the encoder class. |
krogedal | 6:ed47deb76adf | 5 | * |
krogedal | 6:ed47deb76adf | 6 | * @author Simon Krogedal |
krogedal | 6:ed47deb76adf | 7 | * |
krogedal | 6:ed47deb76adf | 8 | * @version 0.1 |
krogedal | 6:ed47deb76adf | 9 | */ |
krogedal | 6:ed47deb76adf | 10 | |
krogedal | 0:441289ea4e29 | 11 | #ifndef KARBOT_MOTOR_CONTROL_H |
krogedal | 0:441289ea4e29 | 12 | #define KARBOT_MOTOR_CONTROL_H |
krogedal | 0:441289ea4e29 | 13 | |
krogedal | 0:441289ea4e29 | 14 | /* Karbot motor control class |
krogedal | 0:441289ea4e29 | 15 | * Written by Simon Krogedal |
krogedal | 0:441289ea4e29 | 16 | * 27/05/21 |
krogedal | 0:441289ea4e29 | 17 | * Team 9 4th Year project |
krogedal | 0:441289ea4e29 | 18 | * |
krogedal | 0:441289ea4e29 | 19 | * for NUCLEO-F401RE |
krogedal | 0:441289ea4e29 | 20 | * |
krogedal | 0:441289ea4e29 | 21 | */ |
krogedal | 0:441289ea4e29 | 22 | |
krogedal | 0:441289ea4e29 | 23 | #include "encoder.h" |
krogedal | 0:441289ea4e29 | 24 | #include "motor.h" |
krogedal | 3:4b6080e86761 | 25 | |
krogedal | 6:ed47deb76adf | 26 | /** Motor Controller Class |
krogedal | 6:ed47deb76adf | 27 | * |
krogedal | 6:ed47deb76adf | 28 | * This class controls the speed of an individual motor, helping the system keep |
krogedal | 3:4b6080e86761 | 29 | * symmetric with asymmetric components. It is based on the encoder class |
krogedal | 3:4b6080e86761 | 30 | * and contains a pointer to a motor object. With two of these the motion |
krogedal | 3:4b6080e86761 | 31 | * controller can drive the robot to the desired points. |
krogedal | 6:ed47deb76adf | 32 | * |
krogedal | 6:ed47deb76adf | 33 | * @author Simon Krogedal |
krogedal | 6:ed47deb76adf | 34 | * |
krogedal | 6:ed47deb76adf | 35 | * Written by Simon Krogedal |
krogedal | 6:ed47deb76adf | 36 | * |
krogedal | 6:ed47deb76adf | 37 | * |
krogedal | 6:ed47deb76adf | 38 | * Team 9 4th Year project |
krogedal | 6:ed47deb76adf | 39 | * |
krogedal | 6:ed47deb76adf | 40 | * |
krogedal | 6:ed47deb76adf | 41 | * for NUCLEO-F401RE |
krogedal | 6:ed47deb76adf | 42 | * |
krogedal | 6:ed47deb76adf | 43 | * @version 0.1 |
krogedal | 6:ed47deb76adf | 44 | * |
krogedal | 3:4b6080e86761 | 45 | */ |
krogedal | 3:4b6080e86761 | 46 | class MotorControl : public encoder { |
krogedal | 6:ed47deb76adf | 47 | |
krogedal | 0:441289ea4e29 | 48 | private: |
krogedal | 6:ed47deb76adf | 49 | |
krogedal | 3:4b6080e86761 | 50 | motor* mot; // motor object |
krogedal | 0:441289ea4e29 | 51 | |
krogedal | 3:4b6080e86761 | 52 | double Kp, // Proportional gain |
krogedal | 3:4b6080e86761 | 53 | Ti, // Integral gain time |
krogedal | 3:4b6080e86761 | 54 | r_speed, // Speed set point |
krogedal | 3:4b6080e86761 | 55 | r_clicks, // Same but in the "encoder language" |
krogedal | 3:4b6080e86761 | 56 | max_speed, // The max speed of the motor (not used) |
krogedal | 3:4b6080e86761 | 57 | output, // PWM output duty cycle |
krogedal | 3:4b6080e86761 | 58 | prev_error; // Previous error, used for intergral control |
krogedal | 3:4b6080e86761 | 59 | |
krogedal | 3:4b6080e86761 | 60 | bool dir, // Drivign direction (not used) |
krogedal | 3:4b6080e86761 | 61 | max_out; // Flag triggered when output is saturated (not used) |
krogedal | 0:441289ea4e29 | 62 | |
krogedal | 6:ed47deb76adf | 63 | /** Get the current tracking error |
krogedal | 6:ed47deb76adf | 64 | * @return Current tracking error |
krogedal | 6:ed47deb76adf | 65 | */ |
krogedal | 6:ed47deb76adf | 66 | double getError(void); |
krogedal | 0:441289ea4e29 | 67 | |
krogedal | 6:ed47deb76adf | 68 | /// Control algorithm, called intermittedly by ticker object |
krogedal | 6:ed47deb76adf | 69 | void algorithm(void); |
krogedal | 0:441289ea4e29 | 70 | |
krogedal | 0:441289ea4e29 | 71 | public: |
krogedal | 6:ed47deb76adf | 72 | /** Creates an instance |
krogedal | 6:ed47deb76adf | 73 | * |
krogedal | 6:ed47deb76adf | 74 | * @param EncoderChanA Encoder channel A pin, set up using internal pullup |
krogedal | 6:ed47deb76adf | 75 | * @param EncoderChanB Encoder channel B pin, set up using internal pullup |
krogedal | 6:ed47deb76adf | 76 | * @param CPR Encoder counts per revolution |
krogedal | 6:ed47deb76adf | 77 | * @param side Left side or right side motor, defines whether counter-clockwise rotation is forwards (left) or backwards (right) (when looking at the robot from outside) |
krogedal | 6:ed47deb76adf | 78 | * @param period Sampling period for control algorithm and speed calculation from encoder readings |
krogedal | 6:ed47deb76adf | 79 | * @param Motor Pointer to the motor object to be controlled |
krogedal | 6:ed47deb76adf | 80 | * @param MaxSpeed Maximum speed of the motor, no set points above this value will be accepted |
krogedal | 6:ed47deb76adf | 81 | * @param kp Proportional control gain |
krogedal | 6:ed47deb76adf | 82 | * @param ti Integral control time |
krogedal | 6:ed47deb76adf | 83 | * @param diameter Wheel diameter in meters |
krogedal | 6:ed47deb76adf | 84 | */ |
krogedal | 6:ed47deb76adf | 85 | MotorControl(PinName EncoderChanA, PinName EncoderChanB, int CPR, encoder::Side side, double period, motor* Motor, double MaxSpeed, double kp, double ti, double diameter); |
krogedal | 6:ed47deb76adf | 86 | |
krogedal | 6:ed47deb76adf | 87 | /** Set the speed set point of the controller |
krogedal | 6:ed47deb76adf | 88 | * This can be done while driving or while stopped, though the update will only be reflected while driving. |
krogedal | 6:ed47deb76adf | 89 | * @param speed Speed set point in m/s |
krogedal | 6:ed47deb76adf | 90 | */ |
krogedal | 6:ed47deb76adf | 91 | void setSpeed(double speed); |
krogedal | 6:ed47deb76adf | 92 | |
krogedal | 6:ed47deb76adf | 93 | /** Start driving |
krogedal | 6:ed47deb76adf | 94 | * Attaches a ticker object to the control algorithm, running it at the frequency specified in the constructor |
krogedal | 6:ed47deb76adf | 95 | */ |
krogedal | 6:ed47deb76adf | 96 | void drive(void); |
krogedal | 6:ed47deb76adf | 97 | |
krogedal | 6:ed47deb76adf | 98 | /** Stops the control algorithm |
krogedal | 6:ed47deb76adf | 99 | * Stops the motors and detaches the ticker callback |
krogedal | 6:ed47deb76adf | 100 | */ |
krogedal | 6:ed47deb76adf | 101 | void stop(void); |
krogedal | 0:441289ea4e29 | 102 | |
krogedal | 6:ed47deb76adf | 103 | /** Drives the motor in open loop |
krogedal | 6:ed47deb76adf | 104 | * No ticket object is attaced, the motors are just activated. |
krogedal | 6:ed47deb76adf | 105 | * Note that this also means new speed set points will not be passed to the motor unless this function is called again! |
krogedal | 6:ed47deb76adf | 106 | */ |
krogedal | 6:ed47deb76adf | 107 | void driveManual(void); |
krogedal | 6:ed47deb76adf | 108 | |
krogedal | 6:ed47deb76adf | 109 | /** Sample the encoders to read speed value |
krogedal | 6:ed47deb76adf | 110 | * This function does not return anything, the speed must then be obtained through getSpeed() |
krogedal | 6:ed47deb76adf | 111 | */ |
krogedal | 6:ed47deb76adf | 112 | void samplecall(void); |
krogedal | 6:ed47deb76adf | 113 | |
krogedal | 6:ed47deb76adf | 114 | /** Set proportional gain |
krogedal | 6:ed47deb76adf | 115 | * This function is useful for Ziegler-Nichols tuning |
krogedal | 6:ed47deb76adf | 116 | * |
krogedal | 6:ed47deb76adf | 117 | * @param k Controller proportional gain |
krogedal | 6:ed47deb76adf | 118 | */ |
krogedal | 3:4b6080e86761 | 119 | void setK(double k); // Set gain (used for Z-N tuning |
krogedal | 6:ed47deb76adf | 120 | |
krogedal | 5:44b2454a5eea | 121 | //void start(void); // This function is overridden to avoid bugs |
krogedal | 6:ed47deb76adf | 122 | |
krogedal | 6:ed47deb76adf | 123 | /** Returns the current duty cycle, useful for debugging |
krogedal | 6:ed47deb76adf | 124 | * @return Current duty cycle |
krogedal | 6:ed47deb76adf | 125 | */ |
krogedal | 3:4b6080e86761 | 126 | double getOutput(void); // Returns current duty cycle |
krogedal | 6:ed47deb76adf | 127 | |
krogedal | 6:ed47deb76adf | 128 | /** Checks whether the output saturation flag is set |
krogedal | 6:ed47deb76adf | 129 | * |
krogedal | 6:ed47deb76adf | 130 | * @return True: Output is saturated |
krogedal | 6:ed47deb76adf | 131 | * @return False: Output is not saturated |
krogedal | 6:ed47deb76adf | 132 | */ |
krogedal | 3:4b6080e86761 | 133 | bool checkFlag(void); // Check whether max out flag is set |
krogedal | 0:441289ea4e29 | 134 | }; |
krogedal | 0:441289ea4e29 | 135 | |
krogedal | 0:441289ea4e29 | 136 | #endif |