Diff: TVDCTRL.cpp

Revision:

1:4d86ec2fe4b1

Parent:

0:276c1dab2d62

Child:

2:9d69f27a3d3b

--- a/TVDCTRL.cpp	Fri Jul 08 05:29:15 2016 +0000
+++ b/TVDCTRL.cpp	Sat Jul 09 12:04:47 2016 +0000
@@ -1,7 +1,191 @@
 #include "TVDCTRL.h"
+#include "MCP4922.h"
+#include "Steering.h"
+
+extern AnalogIn apsP;
+extern AnalogIn apsS;
+extern DigitalOut LED[];
+extern InterruptIn rightMotorPulse;
+extern InterruptIn leftMotorPulse;
+extern DigitalOut MotorPulse[];
+extern MCP4922 mcp;
+
+static Ticker ticker;   //static -> このファイル内でのみ有効
+static Timer timer;
+
+#define apsPVol() (apsP.read() * 3.3f)
+#define apsSVol() (apsS.read() * 3.3f)
+
+const unsigned int APS_MIN_POSITION       =(unsigned int)(0xFFFF/3.3f * 1.0f);     //"正常時"最小入力電圧
+const unsigned int APS_MAX_POSITION       =(unsigned int)(0xFFFF/3.3f * 1.27f);    //"正常時"最大入力電圧
+const unsigned int APS_LOW_VOLTAGE        =(unsigned int)(0xFFFF/3.3f * 0.35f);    //"異常時"最低入力電圧
+const unsigned int APS_HIGH_VOLTAGE       =(unsigned int)(0xFFFF/3.3f * 3.0f);     //"異常時"最高入力電圧
+const unsigned int APS_DEADBAND           =(unsigned int)((APS_MAX_POSITION - APS_MIN_POSITION) * 0.1f);    //APS信号不感帯
+const unsigned int APS_VALID_RANGE        =(APS_MAX_POSITION - APS_MIN_POSITION) - APS_DEADBAND;    //APS信号有効範囲
+
+const unsigned int MCP_REFERENCE          =(unsigned int)(0xFFFF/3.3f * 3.3f);
+const unsigned int DACOUTPUT_MIN          =(unsigned int)(0xFFFF/3.3f * 0.49f);
+const unsigned int DACOUTPUT_MAX          =(unsigned int)(0xFFFF/3.3f * 2.45f);
+const unsigned int DACOUTPUT_VALID_RANGE  =DACOUTPUT_MAX - DACOUTPUT_MIN;
+const unsigned int DACOUTPUT_LIMIT        =(unsigned int)(0xFFFF/3.3f * 0.4f);
 
 struct {
     unsigned int valA:12;
     unsigned int valB:12;
 } mcpData;
 
+unsigned int g_apsP=0, g_apsS=0, g_brake=0;   //現在のセンサ値
+unsigned int rightTire=0, leftTire=0;         //出力タイヤトルク
+unsigned int currentAPS=0;
+unsigned int currentRequestTorque=0;
+unsigned int rightPulseTime=0, leftPulseTime=0;
+
+struct {
+    unsigned int apsUnderVolt;      //aps電圧不足
+    unsigned int apsExceedVolt;     //aps電圧超過
+    unsigned int brakeUnderVolt;    //brake電圧不足
+    unsigned int brakeExceedVolt;   //brake電圧超過
+    unsigned int apsStick;          //aps固着
+} errCounter={0,0,0,0,0};
+
+inline float convertVoltage(unsigned int count)
+{
+    return (count*(3.3f/0xFFFF));
+}
+
+int checkSensorPlausibility(void)
+{
+    int plausibility = 1;
+
+    if(g_apsP < APS_MIN_POSITION) {
+        plausibility = 0;
+        LED[0] = 1;
+    }
+    if(g_apsP > APS_MAX_POSITION) {
+        plausibility = 0;
+        LED[0] = 1;
+    }
+    if(g_apsS < APS_MIN_POSITION) {
+        plausibility = 0;
+        LED[1] = 1;
+    }
+    if(g_apsS > APS_MAX_POSITION) {
+        plausibility = 0;
+        LED[1] = 1;
+    }
+
+    return plausibility;
+}
+
+inline void calcRequestTorque(void)
+{
+    unsigned int currentAPS;
+
+    currentAPS = ((g_apsP>g_apsS) ? g_apsS : g_apsP);   //センサ値は小さい方を採用
+    currentAPS -= APS_MIN_POSITION;     //オフセット修正
+
+    if(currentAPS < APS_DEADBAND)   //デッドバンド内であれば要求トルク->0
+        currentRequestTorque = 0;
+    else
+        currentRequestTorque = (unsigned int)(((float)MAX_MOTOR_TORQUE / APS_VALID_RANGE) * (currentAPS - APS_DEADBAND));
+}
+
+void loadSensorsLPF(void)
+{
+    static unsigned int preApsP=0, preApsS=0;
+
+    preApsP = (unsigned int)(apsP.read_u16()*ratioLPF + preApsP*(1.0f-ratioLPF));
+    preApsS = (unsigned int)(apsS.read_u16()*ratioLPF + preApsS*(1.0f-ratioLPF));
+
+    g_apsP = preApsP;
+    g_apsS = preApsS;
+}
+
+int getVelocity(void)
+{
+    if(rightPulseTime > 100)
+        rightPulseTime = 100;
+    if(leftPulseTime > 100)
+        leftPulseTime = 100;
+
+    //return (int)convPToV_533[(int)((rightPulseTime+leftPulseTime)/2.0f)];
+    return (int)convPToV_533[rightPulseTime];
+}
+
+void countPulseR(void)
+{
+    //Do not use "printf" in interrupt!!!
+    static int preTime=0;
+    int currentTime = timer.read_ms();
+    rightPulseTime = currentTime - preTime;
+    preTime = currentTime;
+}
+
+void countPulseL(void)
+{
+    //Do not use "printf" in interrupt!!!
+    static int preTime=0;
+    int currentTime = timer.read_ms();
+    leftPulseTime = currentTime - preTime;
+    preTime = currentTime;
+}
+
+void generatePulse(void)
+{
+    static bool flag = false;
+    flag = !flag;
+    MotorPulse[0] = MotorPulse[1] = LED[0] = flag;
+}
+
+void driveTVD(void)
+{
+    //仮に等配分のみ
+    mcpData.valA = (unsigned int)(((float)DACOUTPUT_VALID_RANGE / MAX_MOTOR_TORQUE) * currentRequestTorque) + DACOUTPUT_MIN;
+    mcpData.valB = (unsigned int)(((float)DACOUTPUT_VALID_RANGE / MAX_MOTOR_TORQUE) * currentRequestTorque) + DACOUTPUT_MIN;
+
+    mcp.writeA(mcpData.valA);
+    mcp.writeB(mcpData.valB);
+}
+
+void initTVD(void)
+{
+    rightMotorPulse.mode(PullUp);
+    leftMotorPulse.mode(PullUp);
+    rightMotorPulse.fall(&countPulseR);
+    leftMotorPulse.fall(&countPulseL);
+
+    timer.reset();
+    timer.start();
+
+    ticker.attach(&loadSensorsLPF, 0.001);    //サンプリング周期1msec
+    ticker.attach(&generatePulse, 0.025);
+}
+
+
+
+
+//APS信号をモーターコントローラの入力電圧に変換
+//fixVoltageを通してから使うこと
+inline float fixSpecifiedOutputData(float aps)
+{
+    float temp;
+
+    aps = ((aps<APS_MIN_POSITION) ? 0.0f : (aps-APS_MIN_POSITION)); //オフセット修正
+
+    /*
+    if(aps < APS_DEADBAND)
+        temp = 0.0f;
+    else {
+        aps -= APS_DEADBAND;
+        temp = (aps * (DACOUTPUT_VALID_RANGE/(APS_VALID_RANGE)));
+        temp *= LIMIT;
+    }
+
+    temp += DACOUTPUT_MIN;
+    */
+
+    //temp = (aps * (1.1f/(APS_MAX_VOLTAGE-APS_MIN_VOLTAGE)));
+
+    return temp/3.3f;
+}
+