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

--- 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);
 }
 

--- 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
 };
 

--- 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;

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

--- 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();