Aditya Mehrotra
Modified Motor Driver Firmware to include Flash + Thermal
Dependencies: FastPWM3 mbed-dev-STM-lean
Diff: main.cpp
- Revision:
- 73:5676f1232a81
- Parent:
- 72:3163691b0851
- Child:
- 74:fcc5f8e7f0ef
--- a/main.cpp Sun Aug 28 18:01:26 2022 +0000
+++ b/main.cpp Mon Aug 29 16:03:54 2022 +0000
@@ -11,7 +11,7 @@
#define ENCODER_MODE 5
#define INIT_TEMP_MODE 6
-#define VERSION_NUM "1.10"
+#define VERSION_NUM "2.0" //changed to 2.0 for flash storage version
float __float_reg[64]; // Floats stored in flash
@@ -26,7 +26,7 @@
#include "math_ops.h"
#include "current_controller_config.h"
#include "hw_config.h"
-#include "motor_config_U12.h" // need to choose between two files here, one for U10 and one for U12
+//#include "motor_config_U12.h" // need to choose between two files here, one for U10 and one for U12
#include "stm32f4xx_flash.h"
#include "FlashWriter.h"
#include "user_config.h"
@@ -119,6 +119,9 @@
wait_us(10);
printf(" %-4s %-31s %-5s %-6s %-2s\n\r\n\r", "prefix", "parameter", "min", "max", "current value");
wait_us(10);
+ //MOTOR CONTROLLER PARAMS
+ printf("MOTOR CONTROLLER PARAMS\n\r");
+ 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);
@@ -135,12 +138,47 @@
wait_us(10);
printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "c", "Continuous Current (A)", "0", "40.0", I_MAX_CONT);
wait_us(10);
+ //CURRENT CONTROLLER PARAMS
+ printf("CURRENT CONTROLLER PARAMS\n\r");
+ wait_us(10);
printf(" %-4s %-31s %-5s %-6s %f\n\r", "s", "Current Controller K_SCALE", "0", "0.1", K_SCALE); //ADDED --> FOR CURRENT CONTROL GAINS (KP)
wait_us(10);
printf(" %-4s %-31s %-5s %-6s %f\n\r", "d", "Current Controller KI_D", "0", "1.0", KI_D); //ADDED --> FOR CURRENT CONTROL GAINS (KI D-Axis)
wait_us(10);
printf(" %-4s %-31s %-5s %-6s %f\n\r", "q", "Current Controller KI_Q", "0", "1.0", KI_Q); //ADDED --> FOR CURRENT CONTROL GAINS (KI Q-Axis)
wait_us(10);
+ //MOTOR PARAMS
+ printf("MOTOR PARAMS\n\r");
+ wait_us(10);
+ printf(" %-4s %-31s %-5s %-6s %f\n\r", "r", "Motor Phase Resistance (ohms)", "0", "1.0", R_PHASE);
+ wait_us(10);
+ printf(" %-4s %-31s %-5s %-6s %f\n\r", "a", "D-axis inductance (H)", "0", "0.1", L_D);
+ wait_us(10);
+ printf(" %-4s %-31s %-5s %-6s %f\n\r", "e", "Q-axis inductance (H)", "0", "0.1", L_Q);
+ wait_us(10);
+ printf(" %-4s %-31s %-5s %-6s %d\n\r", "n", "Number of Pole Pairs (NPP)", "0", "40", NPP);
+ wait_us(10);
+ //ACTUATOR PARAMS
+ printf("ACTUATOR PARAMS\n\r");
+ wait_us(10);
+ printf(" %-4s %-31s %-5s %-6s %f\n\r", "p", "Torque Constance Nm/peak phase amp", "0", "1.0", KT);
+ wait_us(10);
+ printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "g", "Module Gear Ratio", "0", "20.0", GR);
+ wait_us(10);
+ printf(" %-4s %-31s %-5s %-6s %.2f\n\r", "k", "KT*GR", "0", "20.0", KT_OUT);
+ wait_us(10);
+ printf(" %-4s %-31s %-5s %-6s %f\n\r", "w", "Flux Linkage from KT = = WB*NPP*3/2", "0", "1.0", WB);
+ wait_us(10);
+ //THERMAL PARAMS
+ printf("THERMAL PARAMS\n\r");
+ wait_us(10);
+ printf(" %-4s %-31s %-5s %-6s %.2f\n\r", "x", "Thermal Resistance (K-W/J)", "0", "10.0", R_TH);
+ wait_us(10);
+ printf(" %-4s %-31s %-5s %-6s %f\n\r", "y", "Observer M Matrix (K/J)", "0", "1.0", INV_M_TH);
+ wait_us(10);
+ printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "z", "Ambient Temp @ Calibration (C)", "0", "35.0", T_AMBIENT);
+ wait_us(10);
+ //END CONFIGS
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;
@@ -410,6 +448,39 @@
case 'q': //CURRENT CONTORL KI_Q
KI_Q = fmaxf(fminf(atof(cmd_val), 1.0f), 0.0f);
break;
+ case 'r':
+ R_PHASE = fmaxf(fminf(atof(cmd_val), 1.0f), 0.0f);
+ break;
+ case 'a':
+ L_D = fmaxf(fminf(atof(cmd_val), 0.1f), 0.0f);
+ break;
+ case 'e':
+ L_Q = fmaxf(fminf(atof(cmd_val), 0.1f), 0.0f);
+ break;
+ case 'n':
+ NPP = atoi(cmd_val);
+ break;
+ case 'p':
+ KT = fmaxf(fminf(atof(cmd_val), 1.0f), 0.0f);
+ break;
+ case 'g':
+ GR = fmaxf(fminf(atof(cmd_val), 20.0f), 0.0f);
+ break;
+ case 'k':
+ KT_OUT = fmaxf(fminf(atof(cmd_val), 20.0f), 0.0f);
+ break;
+ case 'w':
+ WB = fmaxf(fminf(atof(cmd_val), 1.0f), 0.0f);
+ break;
+ case 'x':
+ R_TH = fmaxf(fminf(atof(cmd_val), 10.0f), 0.0f);
+ break;
+ case 'y':
+ INV_M_TH = fmaxf(fminf(atof(cmd_val), 1.0f), 0.0f);
+ break;
+ case 'z':
+ T_AMBIENT = fmaxf(fminf(atof(cmd_val), 35.0f), 0.0f);
+ break;
default:
printf("\n\r '%c' Not a valid command prefix\n\r\n\r", cmd_id);
break;
@@ -497,6 +568,7 @@
rxMsg.len = 8;
can.attach(&onMsgReceived);
+ //SETUP ALL FLASH VALUES
if(isnan(E_OFFSET)){E_OFFSET = 0.0f;}
if(isnan(M_OFFSET)){M_OFFSET = 0.0f;}
if(isnan(I_BW) || I_BW==-1){I_BW = 1000;}
@@ -508,9 +580,27 @@
if(isnan(R_NOMINAL) || R_NOMINAL==-1){R_NOMINAL = 0.0f;}
if(isnan(TEMP_MAX) || TEMP_MAX==-1){TEMP_MAX = 125.0f;}
if(isnan(I_MAX_CONT) || I_MAX_CONT==-1){I_MAX_CONT = 14.0f;}
+ //CURRENT CONTROLLER
if(isnan(K_SCALE) || K_SCALE==-1){K_SCALE = 0.00013310f;} //CURRENT CONTROLLER K_SCALE
if(isnan(KI_D) || KI_D==-1){KI_D = 0.0373f;} //CURRENT CONTROLLER KI_D
if(isnan(KI_Q) || KI_Q==-1){KI_Q = 0.0373f;} //CURRENT CONTROLLER KI_Q
+
+ //DEFAULT MOTOR PARAMS ARE FOR THE U12 MODULE BELOW
+ //MOTOR PARAMS
+ if(isnan(R_PHASE) || R_PHASE==-1){R_PHASE = 0.158f;}
+ if(isnan(L_D) || L_D==-1){L_D = 0.000084f;}
+ if(isnan(L_Q) || L_Q==-1){L_Q = 0.000084f;}
+ if(isnan(NPP) || NPP==-1){NPP = 21;}
+ //ACTUATOR PARAMS
+ if(isnan(KT) || KT==-1){KT = 0.1916f;}
+ if(isnan(GR) || GR==-1){GR = 6.0f;}
+ if(isnan(KT_OUT) || KT_OUT==-1){KT_OUT = 1.15f;}
+ if(isnan(WB) || WB==-1){WB = 0.00608f;}
+ //THERMAL OBSERVER
+ if(isnan(R_TH) || R_TH==-1){R_TH = 1.25f;}
+ if(isnan(INV_M_TH) || INV_M_TH==-1){INV_M_TH = 0.02825f;}
+ if(isnan(T_AMBIENT) || T_AMBIENT==-1){T_AMBIENT = 25.0f;}
+
spi.SetElecOffset(E_OFFSET); // Set position sensor offset
spi.SetMechOffset(M_OFFSET);
spi.Sample(1.0f);
@@ -525,7 +615,7 @@
pc.baud(921600); // set serial baud rate
wait(.01);
- pc.printf("\n\r\n\r HobbyKing Cheetah\n\r\n\r");
+ pc.printf("\n\r\n\r MIT Biomimetics Motor Driver\n\r\n\r");
wait(.01);
printf("\n\r Debug Info:\n\r");
printf(" Firmware Version: %s\n\r", VERSION_NUM);
@@ -541,7 +631,7 @@
pc.attach(&serial_interrupt); // attach serial interrupt
- int counter = 0;
+ //int counter = 0;
while(1) {
//drv.print_faults();
wait(.1);