qiu dehua
GT_MOTOR_24NM_V03_PT1000
Dependencies: mbed Hobbyking_Cheetah FastPWM3
Diff: main.cpp
- Revision:
- 55:165927e4f815
- Parent:
- 53:e85efce8c1eb
--- a/main.cpp Thu Aug 08 17:39:43 2019 +0000
+++ b/main.cpp Tue May 10 02:26:27 2022 +0000
@@ -10,7 +10,7 @@
#define SETUP_MODE 4
#define ENCODER_MODE 5
-#define VERSION_NUM "1.9"
+#define VERSION_NUM "0.3"
float __float_reg[64]; // Floats stored in flash
@@ -54,10 +54,13 @@
PositionSensorAM5147 spi(16384, 0.0, NPP);
-volatile int count = 0;
+volatile int counts = 0;
volatile int state = REST_MODE;
volatile int state_change;
+
+volatile int cal_counts = 0;
+
void onMsgReceived() {
//msgAvailable = true;
//printf("%d\n\r", rxMsg.id);
@@ -75,11 +78,17 @@
}
else if(((rxMsg.data[0]==0xFF) & (rxMsg.data[1]==0xFF) & (rxMsg.data[2]==0xFF) & (rxMsg.data[3]==0xFF) * (rxMsg.data[4]==0xFF) & (rxMsg.data[5]==0xFF) & (rxMsg.data[6]==0xFF) & (rxMsg.data[7]==0xFE))){
spi.ZeroPosition();
+ }
+ else if(((rxMsg.data[0]==0xFF) & (rxMsg.data[1]==0xFF) & (rxMsg.data[2]==0xFF) & (rxMsg.data[3]==0xFF) * (rxMsg.data[4]==0xFF) & (rxMsg.data[5]==0xFF) & (rxMsg.data[6]==0xFF) & (rxMsg.data[7]==0xFF))){
+ //Null instruction, which can be used to return data
}
else if(state == MOTOR_MODE){
unpack_cmd(rxMsg, &controller);
}
- pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT);
+ //pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT);
+ //pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech*GR, controller.i_q_filt*KT_OUT); // modify by 20210617
+ //pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech*GR, controller.i_q_filt*KT); // 正式使用 plan A&C, modify by 20210630
+ pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT); // test v?, modify by 20210706
can.write(txMsg);
}
@@ -166,7 +175,6 @@
/// This runs at 40 kHz, regardless of of the mode the controller is in ///
extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
if (TIM1->SR & TIM_SR_UIF ) {
-
///Sample current always ///
ADC1->CR2 |= 0x40000000; // Begin sample and conversion
//volatile int delay;
@@ -177,8 +185,11 @@
controller.adc1_raw = ADC1->DR;
controller.adc3_raw = ADC3->DR;
controller.theta_elec = spi.GetElecPosition();
- controller.theta_mech = (1.0f/GR)*spi.GetMechPosition();
- controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity();
+ //controller.theta_mech = (1.0f/GR)*spi.GetMechPosition();
+ controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity();
+ controller.theta_mech = (1.0f)*spi.GetMechPosition(); // modified on 20210617
+ //controller.dtheta_mech = (1.0f)*spi.GetMechVelocity(); // modified on 20210618
+
controller.dtheta_elec = spi.GetElecVelocity();
controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE; //filter the dc link voltage measurement
///
@@ -199,8 +210,11 @@
case MOTOR_MODE: // Run torque control
if(state_change){
+
enter_torque_mode();
- count = 0;
+
+ counts = 0; //20210222
+
}
else{
/*
@@ -224,9 +238,8 @@
torque_control(&controller);
commutate(&controller, &observer, &gpio, controller.theta_elec); // Run current loop
-
controller.timeout++;
- count++;
+ counts ++;
}
break;
@@ -238,7 +251,7 @@
case ENCODER_MODE:
print_encoder();
break;
- }
+ }
}
TIM1->SR = 0x0; // reset the status register
}
@@ -269,7 +282,7 @@
break;
case 'm':
state = MOTOR_MODE;
- state_change = 1;
+ state_change = 1;
break;
case 'e':
state = ENCODER_MODE;
@@ -358,8 +371,112 @@
}
}
+
+//PT1000用二分法查表计算温度
+//测温范围：-50~150℃
+float RTD_TAB_PT1000[201] = {
+
+803.08,807.05,811.02,814.98,818.95,822.92,826.88,830.84,834.80,838.76, //-50~-41
+842.72,846.68,850.63,854.58,858.54,862.49,866.44,870.39,874.33,878.28, //-40~-31
+882.23,886.17,890.11,894.05,897.99,901.93,905.87,909.80,913.74,917.67, //-30~-21
+921.60,925.54,929.47,933.39,937.32,941.25,945.17,949.10,953.02,956.94, //-20~-11
+960.86,964.78,968.70,972.62,976.53,980.45,984.36,988.27,992.18,996.09, //-10~-1
+1000.00,1003.91,1007.81,1011.72,1015.62,1019.53,1023.43,1027.33,1031.23,1035.13, //0~9
+1039.02,1042.92,1046.81,1050.71,1054.60,1058.49,1062.38,1066.27,1070.16,1074.04, //10~19
+1077.93,1081.81,1085.70,1089.58,1093.46,1097.34,1101.22,1105.10,1108.97,1112.85, //20~29
+1116.72,1120.59,1124.46,1128.34,1132.20,1136.07,1139.94,1143.81,1147.67,1151.53, //30~39
+1155.40,1159.26,1163.12,1166.98,1170.83,1174.69,1178.55,1182.40,1186.25,1190.11, //40~49
+1193.96,1197.81,1201.65,1205.50,1209.35,1213.19,1217.04,1220.88,1224.72,1228.56, //50~59
+1232.40,1236.24,1240.08,1243.91,1247.75,1251.58,1255.41,1259.24,1263.07,1266.90, //60~69
+1270.73,1274.56,1278.38,1282.21,1286.03,1289.85,1293.67,1297.49,1301.31,1305.13, //70~79
+1308.94,1312.76,1316.57,1320.39,1324.20,1328.01,1331.82,1335.62,1339.43,1343.24, //80~89
+1347.04,1350.85,1354.65,1358.45,1362.25,1366.05,1369.85,1373.64,1377.44,1381.23, //90~99
+1385.03,1388.82,1392.61,1396.40,1400.19,1403.97,1407.76,1411.54,1415.33,1419.11, //100~109
+1422.89,1426.67,1430.45,1434.23,1438.01,1441.78,1445.56,1449.33,1453.10,1456.87, //110~119
+1460.64,1464.41,1468.18,1471.95,1475.71,1479.48,1483.24,1487.00,1490.76,1494.52, //120~129
+1498.28,1502.04,1505.79,1509.55,1513.30,1517.06,1520.81,1524.56,1528.31,1532.05, //130~139
+1535.80,1539.55,1543.29,1547.03,1550.78,1554.52,1558.26,1562.00,1565.73,1569.47, //140~149
+1573.21 //150
+};
+
+/********************************************
+功能：采用2分查找法,根据电阻值查表计算温度值
+输入：fR,PT1000的电阻值
+输出：fTemp,测量的温度值
+********************************************/
+float CalTemp(float fR)
+{
+ float fTemp;
+ float fLowR;
+ float fHighR;
+ int iTemp;
+ int i;
+ int Top,Bottom;
+
+ cal_counts=0;
+
+ if(fR < RTD_TAB_PT1000[0]) //电阻值小于表格最小值
+ {
+ return -50;
+ }
+
+ if(fR > RTD_TAB_PT1000[200]) //电阻值大于表格最大值
+ {
+ return 150;
+ }
+
+ Bottom = 0;
+ Top = 200;
+
+ for(i=100;(Top-Bottom)!=1;) //2分法查表
+ {
+ if(fR < RTD_TAB_PT1000[i])
+ {
+ Top = i;
+ i = (Top + Bottom) / 2;
+ }
+ else if(fR > RTD_TAB_PT1000[i])
+ {
+ Bottom = i;
+ i = (Top + Bottom) / 2;
+ }
+ else
+ {
+ iTemp = i - 50;
+ fTemp = (float)iTemp;
+ return fTemp;
+ }
+ cal_counts++;
+ }
+
+ iTemp = i - 50;
+ fLowR = RTD_TAB_PT1000[Bottom];
+ fHighR = RTD_TAB_PT1000[Top];
+ fTemp = (fR - fLowR)/(fHighR - fLowR) + iTemp;
+ return fTemp;
+}
+
+
+
+
+
int main() {
+ //*************************
+ //uint32_t btr;
+ //CAN_TypeDef *can = obj->CanHandle.Instance;
+ /*
+ hcan->Instance->BTR = (uint32_t)((uint32_t)hcan->Init.Mode) | \
+ ((uint32_t)hcan->Init.SJW) | \
+ ((uint32_t)hcan->Init.BS1) | \
+ ((uint32_t)hcan->Init.BS2) | \
+ ((uint32_t)hcan->Init.Prescaler - 1U);
+ */
+
+ //uint32_t i,num;
+ float ii;
+
+
controller.v_bus = V_BUS;
controller.mode = 0;
Init_All_HW(&gpio); // Setup PWM, ADC, GPIO
@@ -411,7 +528,7 @@
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;}
- if(isnan(I_MAX) || I_MAX ==-1){I_MAX=40;}
+ if(isnan(I_MAX) || I_MAX ==-1){I_MAX=30;}
if(isnan(I_FW_MAX) || I_FW_MAX ==-1){I_FW_MAX=0;}
if(isnan(CAN_ID) || CAN_ID==-1){CAN_ID = 1;}
if(isnan(CAN_MASTER) || CAN_MASTER==-1){CAN_MASTER = 0;}
@@ -424,8 +541,9 @@
init_controller_params(&controller);
pc.baud(921600); // set serial baud rate
+ pc.attach(&serial_interrupt); // attach serial interrupt
wait(.01);
- pc.printf("\n\r\n\r HobbyKing Cheetah\n\r\n\r");
+ pc.printf("\n\r\n\r GTGJ MOTOR\n\r\n\r");
wait(.01);
printf("\n\r Debug Info:\n\r");
printf(" Firmware Version: %s\n\r", VERSION_NUM);
@@ -435,8 +553,7 @@
printf(" CAN ID: %d\n\r", CAN_ID);
-
-
+//*********
//printf(" %d\n\r", drv.read_register(DCR));
//wait_us(100);
//printf(" %d\n\r", drv.read_register(CSACR));
@@ -444,23 +561,36 @@
//printf(" %d\n\r", drv.read_register(OCPCR));
//drv.disable_gd();
- pc.attach(&serial_interrupt); // attach serial interrupt
+// pc.attach(&serial_interrupt); // attach serial interrupt
state_change = 1;
- int counter = 0;
+ //**********************************************************************************************
+ //for(i=800;i<1580;i++)
+ for(ii=1600;ii<3150;ii++)
+ {
+ //printf("%d %d %.2f\n\r", ii/2, cal_counts, CalTemp((float)ii/2));
+ printf("%.1f %d %.2f\n\r", ii/2.0f, cal_counts, CalTemp(ii/2.0f));
+ }
+
+ //**********************************************************************************************
+
+// int counter = 0;
while(1) {
drv.print_faults();
wait(.1);
- //printf("%.4f\n\r", controller.v_bus);
+ //printf("%.4f\n\r", controller.v_bus);
/*
if(state == MOTOR_MODE)
{
//printf("%.3f %.3f %.3f\n\r", (float)observer.temperature, (float)observer.temperature2, observer.resistance);
- //printf("%.3f %.3f %.3f %.3f %.3f\n\r", controller.v_d, controller.v_q, controller.i_d_filt, controller.i_q_filt, controller.dtheta_elec);
- //printf("%.3f\n\r", controller.dtheta_mech);
- wait(.002);
+ //printf("%.3f %.3f %.3f %.3f %.3f %.3f\n\r", controller.v_d, controller.v_q, controller.i_d_filt, controller.i_q_filt, controller.i_q_ref,controller.dtheta_elec);
+ //printf("%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n\r", controller.i_a, controller.i_b, controller.i_c,controller.theta_elec,controller.i_d, controller.i_q,controller.i_d_filt, controller.i_q_filt);
+ printf("%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n\r", controller.v_d, controller.v_q, controller.i_d_filt, controller.i_q_filt, controller.i_q_ref,controller.theta_elec,controller.theta_mech, controller.dtheta_mech*GR, controller.i_q_filt*KT_OUT/GR);
+ //printf("%.3f\n\r", controller.dtheta_mech);
+ //printf("%d %.4f %.4f\n\r", spi.GetRawPosition(), spi.GetMechPositionFixed(),spi.GetMechPosition());
+ wait(.005);
}
*/