Simon Krogedal / Karbot_wheel_control

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?

UserRevisionLine numberNew 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