Diff: TVDcontrolDEMO_main.cpp

Revision:

1:4d86ec2fe4b1

Parent:

0:276c1dab2d62

Child:

2:9d69f27a3d3b

--- a/TVDcontrolDEMO_main.cpp	Fri Jul 08 05:29:15 2016 +0000
+++ b/TVDcontrolDEMO_main.cpp	Sat Jul 09 12:04:47 2016 +0000
@@ -1,7 +1,5 @@
 #include "mbed.h"
 #include "TVDCTRL.h"
-#include "APSdefs.h"
-#include "Steering.h"
 #include "MCP4922.h"
 
 ////////////////////////////////////////
@@ -13,8 +11,8 @@
 //12
 //13
 //14
-InterruptIn motorPulse1(p15);
-InterruptIn motorPulse2(p16);
+InterruptIn rightMotorPulse(p15);
+InterruptIn leftMotorPulse(p16);
 
 AnalogIn brake(p17);
 AnalogOut ana(p18);
@@ -29,9 +27,7 @@
 
 DigitalOut LED[] = {LED1, LED2, LED3, LED4};
 //DigitalOut watchDog();
-
-#define apsPVol (apsP * 3.3f)
-#define apsSVol (apsS * 3.3f)
+CAN can(p30, p29);
 
 #define indicateSystem(x)       (indicatorLed.write(x))
 #define shutdownSystem(void)    (shutDown.write(0))
@@ -39,109 +35,6 @@
 #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;
@@ -152,42 +45,16 @@
     SDState.mode(PullUp);
 }
 
-int checkSensorPlausibility(void)
+int main(void)
 {
-    int plausibility = 1;
+    printf("\r\nVersion:TVDCU_Alpha...start!!!!!\r\n");
+
+    initIO();       //IOポート初期化
 
-    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;
+    initTVD();
+
+    while(1) {
+
+        printf("%f\r\n", getVelocity()*3.6f);
     }
-
-    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;
-}
+}
\ No newline at end of file