modified 0511
Dependencies: mbed-dev FastPWM3
Diff: main.cpp
- Revision:
- 55:b97b90d06ffa
- Parent:
- 53:e85efce8c1eb
- Child:
- 56:542e99ed5565
diff -r 59575833d16f -r b97b90d06ffa main.cpp --- 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));