Diff: TVDcontrolDEMO_main.cpp

Revision:

0:276c1dab2d62

Child:

1:4d86ec2fe4b1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TVDcontrolDEMO_main.cpp	Fri Jul 08 05:29:15 2016 +0000
@@ -0,0 +1,193 @@
+#include "mbed.h"
+#include "TVDCTRL.h"
+#include "APSdefs.h"
+#include "Steering.h"
+#include "MCP4922.h"
+
+////////////////////////////////////////
+//IO宣言
+SPI spi(p5,p6,p7);
+MCP4922 mcp(p5,p7,p8);  // MOSI, SCLK, CS
+Serial com2(p9, p10);   //xbeeとかデバッグ用
+DigitalOut cs(p11);
+//12
+//13
+//14
+InterruptIn motorPulse1(p15);
+InterruptIn motorPulse2(p16);
+
+AnalogIn brake(p17);
+AnalogOut ana(p18);
+AnalogIn apsS(p19);     //"S"econdary
+AnalogIn apsP(p20);     //"P"rimary
+DigitalOut indicatorLed(p21);
+DigitalOut shutDown(p22);
+DigitalOut MotorPulse[] = {p23, p24};
+DigitalIn RTDSW(p25);
+DigitalIn SDState(p26);
+Serial com1(p28, p27);  //インパネとの通信に使用するかも知れなくもないかもしれない
+
+DigitalOut LED[] = {LED1, LED2, LED3, LED4};
+//DigitalOut watchDog();
+
+#define apsPVol (apsP * 3.3f)
+#define apsSVol (apsS * 3.3f)
+
+#define indicateSystem(x)       (indicatorLed.write(x))
+#define shutdownSystem(void)    (shutDown.write(0))
+#define bootSystem(void)        (shutDown.write(1))
+#define isPressedRTD(void)      (!RTDSW.read())
+#define isShutdownSystem(void)  (SDState.read())
+
+int checkSensorPlausibility(void);
+void initIO(void);
+
+////////////////////////////////////////
+//センサーサンプリング関係
+#define ratioLPF 0.034f    //CutOff:23.89Hz
+void loadSensorsLPF(void);
+////////////////////////////////////////
+
+/*
+//そのうち実装
+CAN can();
+*/
+
+float apsPSample, apsSSample, brakeSample;
+float fixVoltage(float vol)
+{
+    if(vol > APS_MAX_VOLTAGE)
+        return APS_MAX_VOLTAGE;
+    else if(APS_MIN_VOLTAGE > vol)
+        return APS_MIN_VOLTAGE;
+    else
+        return vol;
+}
+
+//APS信号をモーターコントローラの入力電圧に変換
+//fixVoltageを通してから使うこと
+float fixSpecifiedOutputData(float aps)
+{
+    float temp;
+
+    aps = ((aps<APS_MIN_VOLTAGE) ? 0.0f : (aps-APS_MIN_VOLTAGE)); //オフセット修正
+
+    /*
+    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;
+}
+
+Timer timer;
+
+Ticker ticker1;
+Ticker ticker2;
+
+void generatePulse()
+{
+    static bool flag = false;
+    flag = !flag;
+    MotorPulse[0] = MotorPulse[1] = LED[0] = flag;
+}
+
+int pulseTime = 0;
+
+void countPulse1()
+{
+    //Do not use "printf" in interrupt!!!
+    static int preTime=0;
+    int currentTime = timer.read_ms();
+    pulseTime = currentTime - preTime;
+    preTime = currentTime;
+}
+
+int convertPulseToVelocity(int pulseTime1, int pulseTime2)
+{
+    return (int)convPToV_533[(int)((pulseTime1+pulseTime2)/2.0f)];
+}
+
+int main(void)
+{
+    printf("\r\nVersion:TVDCU_Alpha...start!!!!!\r\n");
+
+    initIO();       //IOポート初期化
+    initIoExp();    //エキスパンダIO初期化
+
+    timer.reset();
+    timer.start();
+
+    //ticker.attach(&loadSensorsLPF, 0.001);    //サンプリング周期1msec
+    ticker1.attach(&generatePulse, 0.001);
+
+    motorPulse1.mode(PullUp);
+    motorPulse1.fall(&countPulse1);
+
+    bootSystem();   //システム起動
+
+    indicateSystem(1);  //System OK!
+
+    while(1) {
+        printf("%f\r\n", convertPulseToVelocity(pulseTime, pulseTime)*3.6f);
+    }
+}
+
+void initIO(void)
+{
+    indicatorLed = 0;
+    shutDown = 0;
+    LED[0] = LED[1] = LED[2] = LED[3] = 0;
+
+    RTDSW.mode(PullUp);
+    SDState.mode(PullUp);
+}
+
+int checkSensorPlausibility(void)
+{
+    int plausibility = 1;
+
+    if(apsPVol < APS_MIN_VOLTAGE) {
+        plausibility = 0;
+        LED[0] = 1;
+    }
+    if(apsPVol > APS_MAX_VOLTAGE) {
+        plausibility = 0;
+        LED[0] = 1;
+    }
+    if(apsSVol < APS_MIN_VOLTAGE) {
+        plausibility = 0;
+        LED[1] = 1;
+    }
+    if(apsSVol > APS_MAX_VOLTAGE) {
+        plausibility = 0;
+        LED[1] = 1;
+    }
+
+    return plausibility;
+}
+
+void loadSensorsLPF(void)
+{
+    //timer.reset();
+
+    static float preApsP, preApsS;
+
+    preApsP = apsPVol*ratioLPF + preApsP*(1.0f-ratioLPF);
+    preApsS = apsSVol*ratioLPF + preApsS*(1.0f-ratioLPF);
+
+//    preApsP = apsPVol;
+//    preApsS = apsSVol;
+
+    apsPSample = preApsP;
+    apsSSample = preApsS;
+}