control

Author: gbr1

Arduino_Robot_Firmware.h

@@ -7,6 +7,7 @@
 #include "encoder.h"
 #include "rgb_led.h"
 #include "dcmotor.h"
+#include "motor_control.h"
 
 
 

dcmotor.h

@@ -19,7 +19,7 @@ class DCmotor{
 
     public:
         DCmotor(const uint32_t _pinA, const uint32_t _chA, const uint32_t _pinB, const uint32_t _chB, 
-                const bool flip=false,TIM_TypeDef * _tim = TIM2, const uint32_t _frequency=20000){
+                const bool flip=false, TIM_TypeDef * _tim = TIM2, const uint32_t _frequency=20000){
             if (!flip){
                 pinA=_pinA;
                 chA=_chA;

examples/motor/motor.ino

@@ -0,0 +1,16 @@
+#include "Arduino_Robot_Firmware.h"
+
+Encoder enc_right(TIM5);
+
+DCmotor motor_right(MOTOR_RIGHT_A,MOTOR_RIGHT_A_CH,MOTOR_RIGHT_B,MOTOR_RIGHT_B_CH);
+
+MotorControl right(&motor_right,0,0,0);
+
+void setup(){
+    enc_right.begin();
+    motor_right.begin();
+}
+
+void loop(){
+    right.test();
+}

motor_control.h

@@ -0,0 +1,107 @@
+#ifndef __MOTOR_CONTROL_H__
+#define __MOTOR_CONTROL_H__
+
+#include "Arduino.h"
+#include "dcmotor.h"
+#include "encoder.h"
+#include "robot_definitions.h"
+
+#define CONTROL_LIMIT 4095
+
+class MotorControl{
+    private:
+        float kp;
+        float ki;
+        float kd;
+        
+        float travel;
+        float reference;
+
+        float measure;
+        float error;
+
+        float ctrl_p;
+        float ctrl_i;
+        float ctrl_d;
+        float actuation;
+
+        DCmotor * motor;
+        Encoder * encoder;
+    public:
+        MotorControl(DCmotor * _motor, Encoder * _encoder, const float _kp, const float _ki, const float _kd){
+            motor = _motor;
+            encoder = _encoder;
+            
+            kp = _kp;
+            ki = _ki;
+            kd = _kd;
+
+            travel=0.0;
+            reference = 0.0;
+
+            measure = 0.0;
+            error = 0.0;
+
+            ctrl_p = 0.0;
+            ctrl_i = 0.0;
+            ctrl_d = 0.0;
+
+            actuation = 0.0;
+        }
+
+        void begin(){
+            motor->begin();
+            encoder->begin();
+            encoder->reset();
+        }
+
+        bool setReference(const float ref){
+            if ((ref<MOTOR_LIMIT)&&(ref>-MOTOR_LIMIT)){
+                reference = ref;
+                return true;
+            }
+            return false;
+        }
+
+        float checkLimits(float value){
+            if (value>CONTROL_LIMIT){
+                return CONTROL_LIMIT;
+            }
+            if (value<-CONTROL_LIMIT){
+                return -CONTROL_LIMIT;
+            }
+            return value;
+        }
+
+        
+
+        void test(){
+            motor->setSpeed(2000);
+            delay(1000);
+            motor->setSpeed(-2000);
+            delay(1000);
+            
+        }
+
+
+
+        void update(){
+            measure = encoder->getCount();
+            encoder->reset();
+            error = reference - measure;
+
+            ctrl_p = kp * error;
+            ctrl_i = checkLimits(ctrl_i+ki*error);
+            ctrl_d = kd * (error-prev_error);
+
+            prev_error = error;
+
+            actuation = checkLimits(ctrl_p+ctrl_i+ctrl_d);
+
+            motor->setSpeed(actuation);
+        }
+
+        
+};
+
+#endif

pinout_definitions.h

@@ -4,35 +4,35 @@
 
 // Right Motor
 #define ENC_RIGHT_TIMER TIM2
-#define ENC_RIGHT_A PA_0
-#define ENC_RIGHT_B PA_1
+#define ENC_RIGHT_A PA0
+#define ENC_RIGHT_B PA1
 #define MOTOR_RIGHT_A PA2
 #define MOTOR_RIGHT_A_CH 3
 #define MOTOR_RIGHT_B PA3
 #define MOTOR_RIGHT_B_CH 4
 
 // Left Motor
 #define ENC_LEFT_TIMER TIM3
-#define ENC_LEFT_A PC_6
-#define ENC_LEFT_B PC_7
+#define ENC_LEFT_A PC6
+#define ENC_LEFT_B PC7
 #define MOTOR_LEFT_A PA15
 #define MOTOR_LEFT_A_CH 1
 #define MOTOR_LEFT_B PB3
 #define MOTOR_LEFT_B_CH 2
 
 
 // Leds
-#define LED_BUILTIN PC_8
-#define LED_1_RED PB_12
-#define LED_1_GREEN PB_13
-#define LED_1_BLUE PB_14
-#define LED_2_RED PC_12
-#define LED_2_GREEN PB_4
-#define LED_2_BLUE PB_5
+#define LED_BUILTIN PC8
+#define LED_1_RED PB12
+#define LED_1_GREEN PB13
+#define LED_1_BLUE PB14
+#define LED_2_RED PC12
+#define LED_2_GREEN PB4
+#define LED_2_BLUE PB5
 
 // Analog RC Servos
-#define SERVO_A PC_9
-#define SERVO_B PA_8
+#define SERVO_A PC9
+#define SERVO_B PA8
 
 
 #endif

robot_definitions.h

@@ -0,0 +1,12 @@
+#ifndef __ROBOT_DEFINITIONS_H__
+#define __ROBOT_DEFINITIONS_H__
+
+#define CONTROL_LIMIT 4096              // PWM resolution
+
+#define MOTOR_LIMIT 100.0               // Mechanical RPM limit speed of used motors
+#define MOTOR_CPR 12.0                  // Resolution of the encoder
+#define MOTOR_GEAR_RATIO 150.0          // Gear ratio of the motor
+
+const float MOTOR_RATIO = MOTOR_CPR*MOTOR_GEAR_RATIO;
+
+#endif