Wheel control software for satellite microcontroller running the motors on the Karbor
Dependencies: mbed ros_lib_melodic
Revision 6:ed47deb76adf, committed 2021-06-08
- Comitter:
- krogedal
- Date:
- Tue Jun 08 20:55:08 2021 +0000
- Parent:
- 5:44b2454a5eea
- Commit message:
- Might work now?
Changed in this revision
diff -r 44b2454a5eea -r ed47deb76adf src/MotorControl.cpp --- a/src/MotorControl.cpp Tue Jun 08 15:04:33 2021 +0000 +++ b/src/MotorControl.cpp Tue Jun 08 20:55:08 2021 +0000 @@ -33,16 +33,16 @@ prev_error = error; } -MotorControl::MotorControl(PinName a, PinName b, int r, bool c, double p, motor* m, double ms, double kp, double ti, double diameter) - : encoder(a, b, r, c, p, diameter), mot(m), max_speed(ms), Kp(kp), Ti(ti) { +MotorControl::MotorControl(PinName EncoderChanA, PinName EncoderChanB, int CPR, encoder::Side side, double period, motor* Motor, double MaxSpeed, double kp, double ti, double diameter) + : encoder(EncoderChanA, EncoderChanB, CPR, side, period, diameter), mot(Motor), max_speed(MaxSpeed), Kp(kp), Ti(ti) { output = 0.1; dir = 1; //default to avoid bugs max_out = 0; //flag set prev_error = 0; } -void MotorControl::setSpeed(double s) { //set speed in m/s. remember to consider the motor max speed - r_speed = s; - r_clicks = s / enc_const; +void MotorControl::setSpeed(double speed) { //set speed in m/s. remember to consider the motor max speed + r_speed = speed; + r_clicks = speed / enc_const; // pc.printf("r_clicks set to %2.2f\n", r_clicks); }
diff -r 44b2454a5eea -r ed47deb76adf src/MotorControl.h --- a/src/MotorControl.h Tue Jun 08 15:04:33 2021 +0000 +++ b/src/MotorControl.h Tue Jun 08 20:55:08 2021 +0000 @@ -1,3 +1,13 @@ +/** + * @file MotorControl.h + * + * Class to control a motor with an encoder, inherits from the encoder class. + * + * @author Simon Krogedal + * + * @version 0.1 + */ + #ifndef KARBOT_MOTOR_CONTROL_H #define KARBOT_MOTOR_CONTROL_H @@ -13,13 +23,30 @@ #include "encoder.h" #include "motor.h" -/* This class controls the speed of an individual motor, helping the system keep + /** Motor Controller Class + * + * This class controls the speed of an individual motor, helping the system keep * symmetric with asymmetric components. It is based on the encoder class * and contains a pointer to a motor object. With two of these the motion * controller can drive the robot to the desired points. + * + * @author Simon Krogedal + * + * Written by Simon Krogedal + * + * + * Team 9 4th Year project + * + * + * for NUCLEO-F401RE + * + * @version 0.1 + * */ class MotorControl : public encoder { + private: + motor* mot; // motor object double Kp, // Proportional gain @@ -33,22 +60,76 @@ bool dir, // Drivign direction (not used) max_out; // Flag triggered when output is saturated (not used) - double getError(void); // Calculate current error + /** Get the current tracking error + * @return Current tracking error + */ + double getError(void); - void algorithm(void); // Control algorithm + /// Control algorithm, called intermittedly by ticker object + void algorithm(void); public: - // Constructor takes encoder pins and constants, and a motor object and contorller gains - MotorControl(PinName a, PinName b, int r, bool c, double p, motor* m, double ms, double kp, double ti, double diameter); + /** Creates an instance + * + * @param EncoderChanA Encoder channel A pin, set up using internal pullup + * @param EncoderChanB Encoder channel B pin, set up using internal pullup + * @param CPR Encoder counts per revolution + * @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) + * @param period Sampling period for control algorithm and speed calculation from encoder readings + * @param Motor Pointer to the motor object to be controlled + * @param MaxSpeed Maximum speed of the motor, no set points above this value will be accepted + * @param kp Proportional control gain + * @param ti Integral control time + * @param diameter Wheel diameter in meters + */ + MotorControl(PinName EncoderChanA, PinName EncoderChanB, int CPR, encoder::Side side, double period, motor* Motor, double MaxSpeed, double kp, double ti, double diameter); + + /** Set the speed set point of the controller + * This can be done while driving or while stopped, though the update will only be reflected while driving. + * @param speed Speed set point in m/s + */ + void setSpeed(double speed); + + /** Start driving + * Attaches a ticker object to the control algorithm, running it at the frequency specified in the constructor + */ + void drive(void); + + /** Stops the control algorithm + * Stops the motors and detaches the ticker callback + */ + void stop(void); - void setSpeed(double s); // Set the speed of the wheel - void drive(void); // Drive at set speet - void stop(void); // Stop motor - void driveManual(void); // Drive at set speed in open loop - void samplecall(void); // Sample encoder and update read speed value + /** Drives the motor in open loop + * No ticket object is attaced, the motors are just activated. + * Note that this also means new speed set points will not be passed to the motor unless this function is called again! + */ + void driveManual(void); + + /** Sample the encoders to read speed value + * This function does not return anything, the speed must then be obtained through getSpeed() + */ + void samplecall(void); + + /** Set proportional gain + * This function is useful for Ziegler-Nichols tuning + * + * @param k Controller proportional gain + */ void setK(double k); // Set gain (used for Z-N tuning + //void start(void); // This function is overridden to avoid bugs + + /** Returns the current duty cycle, useful for debugging + * @return Current duty cycle + */ double getOutput(void); // Returns current duty cycle + + /** Checks whether the output saturation flag is set + * + * @return True: Output is saturated + * @return False: Output is not saturated + */ bool checkFlag(void); // Check whether max out flag is set };
diff -r 44b2454a5eea -r ed47deb76adf src/encoder.cpp --- a/src/encoder.cpp Tue Jun 08 15:04:33 2021 +0000 +++ b/src/encoder.cpp Tue Jun 08 20:55:08 2021 +0000 @@ -20,18 +20,18 @@ PinName chanA, PinName chanB, int CPR, - bool c, - double p, + Side side, + double Period, double diameter ) : channelA_(chanA, PullUp), channelB_(chanB, PullUp), - c_d(c), - period(p) { + pulsesPerRev_(CPR), + side_(side), + period(Period) { enc_const = ((diameter * PI) / ((double)(4 * CPR))); pulses_ = 0; - pulsesPerRev_ = CPR; tot_clicks = 0; click_rate = 0; temp_tot = 0; @@ -148,7 +148,7 @@ double encoder::getClicks(void) { // test_sample(); - if(c_d) + if(side_ == Left) return click_rate; else return -click_rate; @@ -156,7 +156,7 @@ double encoder::getSpeed(void) { double s = click_rate * enc_const; //angular frequency = 2pi*CPS/CPR and v = omega*r - if(c_d) + if(side_ == Left) return s; else return -s; @@ -165,7 +165,7 @@ double encoder::getDistance(void) { //calculates total distance and returns it // test_sample(); double d = ((double)click_store * enc_const); - if(c_d) + if(side_ == Left) return d; else return -d; @@ -182,10 +182,11 @@ void encoder::distRst(void) { int temp; - if(c_d) + if(side_ == Left) temp = tot_clicks; else temp = -tot_clicks; + if(temp > click_store) click_store = temp; tot_clicks = 0;
diff -r 44b2454a5eea -r ed47deb76adf src/encoder.h --- a/src/encoder.h Tue Jun 08 15:04:33 2021 +0000 +++ b/src/encoder.h Tue Jun 08 20:55:08 2021 +0000 @@ -22,12 +22,10 @@ /** encoder Class * - * Class for measeuring quadrature magnetic encoders requiring internal pullups + * Class for measuring quadrature magnetic encoders requiring internal pullups * * @author Simon Krogedal * - * Written by Simon Krogedal - * * 27/05/2021 * * Team 9 4th Year project @@ -40,56 +38,26 @@ */ class encoder { - private: - int tot_clicks, temp_tot; // clicks since last distance reset - double click_rate, click_store;// clickrate - bool c_d; // left or right bool - - /** - * Update the pulse count. - * - * Called on every rising/falling edge of channels A/B. - * - * Reads the state of the channels and determines whether a pulse forward - * or backward has occured, updating the count appropriately. - */ - void encode(void); - - InterruptIn channelA_; - InterruptIn channelB_; + public: - int pulsesPerRev_; - int prevState_; - int currState_; - - volatile int pulses_; - - protected: - - Ticker sampler; /// ticker object to sample speed - double period, enc_const; /// sampling period and wheel constant - - /// Sample function called by ticker object - void sample_func(void); - - /** Get wheel speed - * @returns Wheel speed in clicks/second + /** Direction enumerator */ - double getClicks(void); /// returns clickrate - - public: + typedef enum { + Left, // Left side motor, CCW rotation is forward + Right // Right side motor, CW rotation is forward + } Side; /** Create an instance * * @param chanA Encoder channel A pin * @param chanB Encoder channel B pin * @param CPR Encoder clicks per revolution - * @param c Boolean flag to invert speeds, used to correct for left and right motors - * @param p Sampling period of the wheel speed + * @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) + * @param Period Sampling period of the wheel speed * @note Too short samling period gives a noisy reading, too long and dynamics are lost - * @param diameter Wheel diameter + * @param diameter Wheel diameter in meters */ - encoder(PinName chanA, PinName chanB, int CPR, bool c, double p, double diameter); + encoder(PinName chanA, PinName chanB, int CPR, Side side, double Period, double diameter); /** Get wheel speed * @returns Wheel speed in meters/second @@ -134,6 +102,45 @@ */ int getPulses(void); + private: + int tot_clicks, temp_tot; // clicks since last distance reset + double click_rate, click_store;// clickrate + bool c_d; // left or right bool + + /** + * Update the pulse count. + * + * Called on every rising/falling edge of channels A/B. + * + * Reads the state of the channels and determines whether a pulse forward + * or backward has occured, updating the count appropriately. + */ + void encode(void); + + InterruptIn channelA_; + InterruptIn channelB_; + + int pulsesPerRev_; + int prevState_; + int currState_; + + volatile int pulses_; + + Side side_; + + protected: + + Ticker sampler; /// ticker object to sample speed + double period, enc_const; /// sampling period and wheel constant + + /// Sample function called by ticker object + void sample_func(void); + + /** Get wheel speed + * @returns Wheel speed in clicks/second + */ + double getClicks(void); + }; #endif \ No newline at end of file
diff -r 44b2454a5eea -r ed47deb76adf src/main.cpp --- a/src/main.cpp Tue Jun 08 15:04:33 2021 +0000 +++ b/src/main.cpp Tue Jun 08 20:55:08 2021 +0000 @@ -1,4 +1,6 @@ /** @file main.cpp + * @author Simon Krogedal + * @version 0.1 */ /* Karbot motor controller @@ -32,10 +34,18 @@ #define R_MOTOR_DIR PB_10 #define L_MOTOR_DIR PA_9 -#define R_ENC_PIN_A PB_5 -#define R_ENC_PIN_B PB_4 +#define R_ENC_PIN_A PB_5 // Blue +#define R_ENC_PIN_B PB_4 // Purple #define L_ENC_PIN_A PB_3 #define L_ENC_PIN_B PC_13 + +/* These pin definitions were used in ESP, it might be smart to go use those +#define L_ENC_PIN_A PA_14 +#define L_ENC_PIN_B PB_15 +#define R_ENC_PIN_A PA_13 +#define R_ENC_PIN_B PB_1 +*/ + #define BAT_A_PIN PA_0 #define BAT_B_PIN PA_1 @@ -45,7 +55,7 @@ //VARIABLES -#define MOTOR_FREQ 25000.0 // 25 kHz, freq of motor PWM +#define MOTOR_FREQ 20000.0 // 20 kHz, freq of motor PWM, max for driver board #define MOTOR_TS 0.05 // Sample time, ie the time between every PID calculation #define MOTOR_KP 0.00002 // Loop motor gain #define MOTOR_TI 200 // Integral motor gain @@ -75,8 +85,8 @@ motor leftMot (L_MOTOR_PIN, L_MOTOR_DIR, 1/MOTOR_FREQ); motor rightMot (R_MOTOR_PIN, R_MOTOR_DIR, 1/MOTOR_FREQ); -MotorControl leftCtrl (L_ENC_PIN_A, L_ENC_PIN_B, ENCODER_CPR, 1, MOTOR_TS, &leftMot, L_MAX_SPEED, MOTOR_KP, MOTOR_TI, WHEEL_RAD); -MotorControl rightCtrl (R_ENC_PIN_A, R_ENC_PIN_B, ENCODER_CPR, 0, MOTOR_TS, &rightMot, R_MAX_SPEED, MOTOR_KP, MOTOR_TI, WHEEL_RAD); +MotorControl leftCtrl (L_ENC_PIN_A, L_ENC_PIN_B, ENCODER_CPR, encoder::Left, MOTOR_TS, &leftMot, L_MAX_SPEED, MOTOR_KP, MOTOR_TI, WHEEL_RAD); +MotorControl rightCtrl (R_ENC_PIN_A, R_ENC_PIN_B, ENCODER_CPR, encoder::Right, MOTOR_TS, &rightMot, R_MAX_SPEED, MOTOR_KP, MOTOR_TI, WHEEL_RAD); // Callback function for a twist message given by the motion controller void messageCb(const geometry_msgs::Twist& msg); @@ -107,9 +117,12 @@ // Set up joint state message - //wheels_msg.name = ["Left wheel","Right wheel"]; - wheels_msg.name[0] = "Left wheel"; - wheels_msg.name[1] = "Right wheel"; + wheels_msg.name_length = 2; // In these libs, lenght must be set + wheels_msg.velocity_length = 2; // manually for each term + + // Name joints + char *jointNames[] = {"Left wheel","Right wheel"}; + wheels_msg.name = jointNames; // Set up battery messages batA_msg.power_supply_technology = 2; @@ -180,8 +193,8 @@ nh.advertise(right_wheel_pub); nh.loginfo("Setup complete"); - leftCtrl.start(); - rightCtrl.start(); + //leftCtrl.start(); + //rightCtrl.start(); // Main loop while(true) { @@ -209,8 +222,8 @@ batB_pub.publish(&batB_msg); wheels_pub.publish(&wheels_msg); - left_wheel_pub.publish(@left_wheel_speed_msg); - right_wheel_pub.publish(@right_wheel_speed_msg); + left_wheel_pub.publish(&left_wheel_speed_msg); + right_wheel_pub.publish(&right_wheel_speed_msg); nh.spinOnce();