Diff: TVDCTRL.cpp

Revision:

19:571a4d00b89c

Parent:

18:b7c362c8f0fd

Child:

20:3c5061281a7a

--- a/TVDCTRL.cpp	Wed Aug 10 11:21:47 2016 +0000
+++ b/TVDCTRL.cpp	Wed Aug 17 14:17:14 2016 +0000
@@ -8,15 +8,14 @@
 extern DigitalOut LED[];
 extern InterruptIn rightMotorPulse;
 extern InterruptIn leftMotorPulse;
-extern DigitalOut MotorPulse[];
 extern MCP4922 mcp;
 extern Serial pc;
+extern AnalogOut STR2AN;
 
 Timer RightPulseTimer;
 Timer LeftPulseTimer;
 Ticker ticker1;
 Ticker ticker2;
-Ticker ticker3;
 
 #define myAbs(x)    ((x>0)?(x):(-(x)))
 
@@ -300,13 +299,6 @@
     return (float)(M_PI*TIRE_DIAMETER / ((avePulseTime/1000000.0)*TVD_GEAR_RATIO));
 }
 
-void generatePulse(void)
-{
-    static bool flag = false;
-    flag = !flag;
-    MotorPulse[0] = MotorPulse[1] = LED[0] = flag;
-}
-
 int distributeTorque(float steering)
 {
     int disTrq = 0;
@@ -314,11 +306,17 @@
 
     if(steering < deadband)
         disTrq = 0;
-    else {
+    else if(steering < M_PI*0.5){
+        steering -= deadband;
+        disTrq = (int)(MAX_DISTRIBUTION_TORQUE / (M_PI*0.5 - deadband) * steering);
+    }
+    else
+        disTrq = MAX_DISTRIBUTION_TORQUE;
+/*    else {
         steering -= deadband;
         disTrq = (int)(MAX_DISTRIBUTION_TORQUE / (M_PI - deadband) * steering);
     }
-    //pc.printf("%d\r\n", disTrq);
+*/    //pc.printf("%d\r\n", disTrq);
 
     return disTrq;
 }
@@ -364,7 +362,7 @@
 
     outputVoltage += DACOUTPUT_MIN;   //最低入力電圧でかさ上げ
 
-    preOutputVol = (int)(outputVoltage*0.1 + preOutputVol*0.9);
+    preOutputVol = (int)(outputVoltage*0.2676 + preOutputVol*0.7324); //cutoff:5Hz
 
     //printf("%d\r\n", (int)(0xFFF*((double)preOutputVol/0xFFFF)));
 
@@ -445,6 +443,8 @@
     }
 
     indicateSystem(readyToDriveFlag | (errCounter.brakeOverRide > ERRCOUNTER_DECISION));
+//    LED[0] = readyToDriveFlag | (errCounter.brakeOverRide > ERRCOUNTER_DECISION)
+    LED[0] = isPressedRTD();
 
     requestTorque=calcRequestTorque();  //ドライバー要求トルク取得
 
@@ -455,7 +455,7 @@
     //デバッグ中
     //distributionTrq = 0;
     
-    if(requestTorque < MIN_INNERWHEEL_TORQUE) {
+    if(requestTorque < MIN_INNERWHEEL_MOTOR_TORQUE) {
         torqueHigh = torqueLow = requestTorque;     //内輪側モーター最低トルクより小さい要求トルクなら等配分
     } else {
         if(requestTorque + distributionTrq > MAX_OUTPUT_TORQUE)  //片モーター上限時最大値にクリップ
@@ -463,15 +463,15 @@
         else
             torqueHigh = requestTorque + distributionTrq;
             
-        if(requestTorque - distributionTrq < MIN_INNERWHEEL_TORQUE) {
-            torqueLow = MIN_INNERWHEEL_TORQUE;      //内輪最低トルクにクリップ
-            torqueHigh = (int)((requestTorque-MIN_INNERWHEEL_TORQUE)*2.0) + MIN_INNERWHEEL_TORQUE;   //片モーター下限値時，トルク高側のモーターも出力クリップ
+        if(requestTorque - distributionTrq < MIN_INNERWHEEL_MOTOR_TORQUE) {
+            torqueLow = MIN_INNERWHEEL_MOTOR_TORQUE;      //内輪最低トルクにクリップ
+            torqueHigh = (int)((requestTorque-MIN_INNERWHEEL_MOTOR_TORQUE)*2.0) + MIN_INNERWHEEL_MOTOR_TORQUE;   //片モーター下限値時，トルク高側のモーターも出力クリップ
         } else
             torqueLow = requestTorque - distributionTrq;
     }
 
     //printf("%d %d\r\n", torqueLow, torqueHigh);
-
+    
     if(getSteerDirection()==1) {
         //steer left
         McpData.valA = calcTorqueToVoltage(torqueHigh, RIGHT_MOTOR);
@@ -501,13 +501,12 @@
     LeftPulseTimer.start();
 
     ticker1.attach(&loadSensorsISR, 0.01f);    //サンプリング周期10msec
-    //ticker2.attach(&generatePulse, 0.03f);
-    ticker3.attach(&getPulseTimeISR, 0.01f);
+    ticker2.attach(&getPulseTimeISR, 0.01f);
 
     mcp.writeA(0);   //右モーター
     mcp.writeB(0);   //左モーター
     
-    printf("MAX OUTPUT TORQUE:\t%1.2f[Nm]\r\n", 45.0/0xFFFF * MAX_OUTPUT_TORQUE);
+    printf("MAX OUTPUT TORQUE:\t\t%1.2f[Nm]\r\n", 45.0/0xFFFF * MAX_OUTPUT_TORQUE);
     printf("MAX DISTRIBUTION TORQUE:\t%1.2f[Nm]\r\n", 45.0/0xFFFF * MAX_DISTRIBUTION_TORQUE);
-    printf("MIN INNERWHEEL-MOTOR TORQUE:\t%1.2f[Nm]\r\n", 45.0/0xFFFF * MIN_INNERWHEEL_TORQUE);
+    printf("MIN INNERWHEEL-MOTOR TORQUE:\t%1.2f[Nm]\r\n", 45.0/0xFFFF * MIN_INNERWHEEL_MOTOR_TORQUE);
 }