Diff: TVDCTRL.cpp

Revision:

8:a22aec357a64

Parent:

7:ad013d88a539

Child:

9:220e4e77e056

--- a/TVDCTRL.cpp	Wed Jul 27 03:30:35 2016 +0000
+++ b/TVDCTRL.cpp	Wed Jul 27 04:45:07 2016 +0000
@@ -10,6 +10,7 @@
 extern InterruptIn leftMotorPulse;
 extern DigitalOut MotorPulse[];
 extern MCP4922 mcp;
+extern Serial pc;
 
 Timer RightPulseTimer;
 Timer LeftPulseTimer;
@@ -309,7 +310,7 @@
 //トルク値線形補間関数
 inline int interpolateLinear(int torque, int currentMaxTorque)
 {
-    return (int)(((double)(DACOUTPUT_MAX-LINEAR_REGION_VOLTAGE)/(currentMaxTorque-LINEAR_REGION_TORQUE)) * torque) + LINEAR_REGION_VOLTAGE-DACOUTPUT_MIN;
+    return (int)(((double)(DACOUTPUT_MAX-LINEAR_REGION_VOLTAGE)/(currentMaxTorque-LINEAR_REGION_TORQUE)) * (torque-LINEAR_REGION_TORQUE)) + LINEAR_REGION_VOLTAGE-DACOUTPUT_MIN;
 }
 
 unsigned int calcTorqueToVoltage(int torque, SelectMotor rl)
@@ -317,7 +318,7 @@
     int outputVoltage=0;
     int rpm=0;
     int currentMaxTorque=0;
-
+    
     if(torque <= LINEAR_REGION_TORQUE) {         //要求トルク<=2.5Nmの時
         outputVoltage = (int)((double)(LINEAR_REGION_VOLTAGE-DACOUTPUT_MIN)/LINEAR_REGION_TORQUE * torque);
     } else {
@@ -363,8 +364,10 @@
     }
 
     outputVoltage += DACOUTPUT_MIN;   //最低入力電圧でかさ上げ
-
-    return (unsigned int)(FIX_DACOUTFORM * outputVoltage);  //DACの分解能に適応(16bit->12bit)
+    
+    printf("%d\r\n", (unsigned int)(0xFFF*((double)outputVoltage/DACOUTPUT_MAX)));
+    
+    return (unsigned int)((double)0xFFF/DACOUTPUT_MAX * outputVoltage);  //DACの分解能に適応(16bit->12bit)
 }
 
 int calcRequestTorque(void)
@@ -426,12 +429,12 @@
 
     if(getSteerDirection()) {
         //steer left
-        McpData.valA = calcTorqueToVoltage(torqueHigh, RIGHT_MOTOR);
-        McpData.valB = calcTorqueToVoltage(torqueLow, LEFT_MOTOR);
+        McpData.valA = calcTorqueToVoltage(requestTorque, RIGHT_MOTOR);
+        McpData.valB = calcTorqueToVoltage(requestTorque, LEFT_MOTOR);
     } else {
         //steer right
-        McpData.valA = calcTorqueToVoltage(torqueLow, RIGHT_MOTOR);
-        McpData.valB = calcTorqueToVoltage(torqueHigh, LEFT_MOTOR);
+        McpData.valA = calcTorqueToVoltage(requestTorque, RIGHT_MOTOR);
+        McpData.valB = calcTorqueToVoltage(requestTorque, LEFT_MOTOR);
     }
 
     mcp.writeA(McpData.valA);   //右モーター