Diff: main.cpp

Revision:

2:916e20f507f2

Parent:

1:30696e4d196b

--- a/main.cpp	Wed Aug 11 09:15:10 2010 +0000
+++ b/main.cpp	Wed Nov 07 17:05:37 2018 +0000
@@ -1,16 +1,111 @@
-#include "QEI.h"
+ 
+/***************************< File comment >***************************/  
+/* project:   Lamborghini48                                           */ 
+/* project description : mobile robot class                           */ 
+/*--------------------------------------------------------------------*/ 
+/* version : 0.1                                                      */ 
+/* board : NUCLEO-F411RE                                              */ 
+/* detail : DC motor control                                          */
+/*--------------------------------------------------------------------*/ 
+/* create : 26/10/2018                                                */ 
+/* programmer : Worasuchad Haomachai                                  */ 
+/**********************************************************************/ 
+ 
+/*--------------------------------------------------------------------*/ 
+/*                           Include file                             */ 
+/*--------------------------------------------------------------------*/
+#include "math.h"
+#include "QEI.h"                        // https://os.mbed.com/cookbook/QEI
 
 Serial pc(USBTX, USBRX);
+
 //Use X4 encoding.
 //QEI wheel(p29, p30, NC, 624, QEI::X4_ENCODING);
 //Use X2 encoding by default.
-QEI wheel (p29, p30, NC, 624);
+QEI wheel (D2, D3, NC, 200, QEI::X4_ENCODING);
+
+/*--------------------------------------------------------------------*/ 
+/*                         Global variable                            */ 
+/*--------------------------------------------------------------------*/
+PwmOut pwm(D5);                         // this is the PWM pin for the motor for how much we move it to correct for its error
+DigitalOut dir1(D6);                    //these pins are to control the direction of the motor (clockwise/counter-clockwise)
+DigitalOut dir2(D7);
+
+double angle = 0;                       //set the angles
 
-int main() {
+double setpoint = 360;                  // 3072;        // I am setting it to move through 100 degrees
+double Kp = 0.05;                       // 0.015;       // you can set these constants however you like depending on trial & error
+double Ki = 0.001;                        // 0.000071;    // 0.000109;
+double Kd = 0.0;                        // 0.93866;     // 0.1009;
+
+float last_error = 0;
+float _error = 0;
+float changeError = 0;
+float totalError = 0;
+float pidTerm = 0;
+float pidTerm_scaled = 0;               // if the total gain we get is not in the PWM range we scale it down so that it's not bigger than |255|
 
-    while(1){
-        wait(0.1);
-        pc.printf("Pulses is: %i\n", wheel.getPulses());
+/*--------------------------------------------------------------------*/ 
+/*                           prototype fun                            */ 
+/*--------------------------------------------------------------------*/
+void PIDcalculation();              // find PID value
+
+/*--------------------------------------------------------------------*/ 
+/*                              main                                  */ 
+/*--------------------------------------------------------------------*/   
+int main() {
+    pc.baud(115200);
+    while(1)
+    {
+        PIDcalculation();               // find PID value
+        
+        if (angle < setpoint) 
+        {
+            dir1 = 1;                   // Forward motion
+            dir2 = 0;
+        } 
+        else 
+        {
+            dir1 = 0;                   // Reverse motion
+            dir2 = 1;
+        }
+    
+        pwm.write(pidTerm_scaled / 255.00f);
+        
+        // Serial.println("WHEEL ANGLE:");
+        pc.printf("%.3f\t\t", setpoint);
+        pc.printf("%.3f\n\r", angle);
+        //pc.printf("%d\n\r", wheel.getPulses());
     }
 
 }
+/*--------------------------------------------------------------------*/ 
+/*                              PID                                   */ 
+/*--------------------------------------------------------------------*/ 
+void PIDcalculation(){
+    /* 
+    *  angle = ( 0.1171875 * count); // count to angle conversion = 360/(64*12*4)
+    *  reduction ratio: 64:1 , pulses per revolution: 12CPR
+    */
+    
+    angle = ( 0.004591 * wheel.getPulses());        // 360/(98*200*4)
+    _error = setpoint - angle;
+    
+    changeError = _error - last_error;              //derivative term
+    totalError += _error;                           //accumalate errors to find integral term
+    pidTerm = (Kp * _error) + (Ki * totalError) + (Kd * changeError);//total gain
+    
+    // pidTerm = constrain(pidTerm, -255, 255);     //constraining to appropriate value
+    if(pidTerm > 255)
+    {
+      pidTerm = 255;
+    }
+    else if(pidTerm < -255)
+    {
+      pidTerm = -255;  
+    }
+    
+    pidTerm_scaled = abs(pidTerm);                  //make sure it's a positive value
+    
+    last_error = _error;
+}