Diff: main.cpp

Revision:

55:b97b90d06ffa

Parent:

53:e85efce8c1eb

Child:

56:542e99ed5565

--- a/main.cpp	Thu Aug 08 17:39:43 2019 +0000
+++ b/main.cpp	Tue May 25 11:43:53 2021 +0000
@@ -13,7 +13,8 @@
 #define VERSION_NUM "1.9"
 
 
-float __float_reg[64];                                                          // Floats stored in flash
+//float __float_reg[64];                                                          // Floats stored in flash
+float __float_reg[67]; // Floats stored in flash(add three floats: kp, ki, kd)
 int __int_reg[256];                                                             // Ints stored in flash.  Includes position sensor calibration lookup table
 
 #include "mbed.h"
@@ -114,7 +115,7 @@
     wait_us(10);
     printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "b", "Current Bandwidth (Hz)", "100", "2000", I_BW);
     wait_us(10);
-    printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "i", "CAN ID", "0", "127", CAN_ID);
+    printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "o", "CAN ID", "0", "127", CAN_ID);
     wait_us(10);
     printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "m", "CAN Master ID", "0", "127", CAN_MASTER);
     wait_us(10);
@@ -124,6 +125,14 @@
     wait_us(10);
     printf(" %-4s %-31s %-5s %-6s %d\n\r", "t", "CAN Timeout (cycles)(0 = none)", "0", "100000", CAN_TIMEOUT);
     wait_us(10);
+    
+    printf(" %-4s %-31s %-5s %-6s %.2f\n\r", "p", "MOTOR_KP", "0.0", "500.0", MOTOR_KP);
+    wait_us(10);
+    printf(" %-4s %-31s %-5s %-6s %.2f\n\r", "i", "MOTOR_KI", "0.0", "10.0", MOTOR_KI);
+    wait_us(10);
+    printf(" %-4s %-31s %-5s %-6s %.2f\n\r", "d", "MOTOR_KD", "0.0", "5.0", MOTOR_KD);
+    wait_us(10);
+    
     printf("\n\r To change a value, type 'prefix''value''ENTER'\n\r i.e. 'b1000''ENTER'\n\r\n\r");
     wait_us(10);
     state_change = 0;
@@ -301,7 +310,7 @@
                     case 'b':
                         I_BW = fmaxf(fminf(atof(cmd_val), 2000.0f), 100.0f);
                         break;
-                    case 'i':
+                    case 'o':
                         CAN_ID = atoi(cmd_val);
                         break;
                     case 'm':
@@ -316,6 +325,15 @@
                     case 't':
                         CAN_TIMEOUT = atoi(cmd_val);
                         break;
+                    case 'p':
+                        MOTOR_KP = fmaxf(fminf(atof(cmd_val), KP_MAX), KP_MIN);;
+                        break;
+                    case 'i':
+                        MOTOR_KI = fmaxf(fminf(atof(cmd_val), KI_MAX), KI_MIN);;
+                        break;
+                    case 'd':
+                        MOTOR_KD = fmaxf(fminf(atof(cmd_val), KD_MAX), KD_MIN);
+                        break;
                     default:
                         printf("\n\r '%c' Not a valid command prefix\n\r\n\r", cmd_id);
                         break;
@@ -416,6 +434,9 @@
     if(isnan(CAN_ID) || CAN_ID==-1){CAN_ID = 1;}
     if(isnan(CAN_MASTER) || CAN_MASTER==-1){CAN_MASTER = 0;}
     if(isnan(CAN_TIMEOUT) || CAN_TIMEOUT==-1){CAN_TIMEOUT = 0;}
+    if(isnan(MOTOR_KP) || MOTOR_KP==-1){MOTOR_KP = 50;}
+    if(isnan(MOTOR_KI) || MOTOR_KI==-1){MOTOR_KI = 0;}
+    if(isnan(MOTOR_KD) || MOTOR_KD==-1){MOTOR_KD = 1.25;}
     spi.SetElecOffset(E_OFFSET);                                                // Set position sensor offset
     spi.SetMechOffset(M_OFFSET);
     int lut[128] = {0};
@@ -434,7 +455,12 @@
     printf(" Output Zero Position:  %.4f\n\r", M_OFFSET);
     printf(" CAN ID:  %d\n\r", CAN_ID);
     
-
+    /*----- set controller parameters -----*/
+    controller.kp = MOTOR_KP;
+    controller.ki = MOTOR_KI;
+    controller.kd = MOTOR_KD;
+    printf("PID controller parameters \n\r");
+    printf("KP: %.3f, KI: %.3f, KD: %.3f \n\r", controller.kp, controller.ki, controller.kd);
 
 
     //printf(" %d\n\r", drv.read_register(DCR));