Diff: TVDcontrolDEMO_main.cpp

Revision:

2:9d69f27a3d3b

Parent:

1:4d86ec2fe4b1

Child:

3:821e2f07a260

--- a/TVDcontrolDEMO_main.cpp	Sat Jul 09 12:04:47 2016 +0000
+++ b/TVDcontrolDEMO_main.cpp	Sun Jul 24 02:48:39 2016 +0000
@@ -1,7 +1,10 @@
 #include "mbed.h"
 #include "TVDCTRL.h"
+#include "Steering.h"
 #include "MCP4922.h"
 
+Serial pc(USBTX, USBRX); // tx, rx
+
 ////////////////////////////////////////
 //IO宣言
 SPI spi(p5,p6,p7);
@@ -13,7 +16,6 @@
 //14
 InterruptIn rightMotorPulse(p15);
 InterruptIn leftMotorPulse(p16);
-
 AnalogIn brake(p17);
 AnalogOut ana(p18);
 AnalogIn apsS(p19);     //"S"econdary
@@ -35,6 +37,8 @@
 #define isPressedRTD(void)      (!RTDSW.read())
 #define isShutdownSystem(void)  (SDState.read())
 
+Timer timer;
+
 void initIO(void)
 {
     indicatorLed = 0;
@@ -43,18 +47,45 @@
 
     RTDSW.mode(PullUp);
     SDState.mode(PullUp);
+    
+    indicateSystem(1);
+    bootSystem();
 }
 
 int main(void)
 {
-    printf("\r\nVersion:TVDCU_Alpha...start!!!!!\r\n");
+    pc.baud(115200);
+    
+    printf("\r\nVersion:TVDcontrolDEMO...start!!!!!\r\n");
 
     initIO();       //IOポート初期化
 
     initTVD();
 
+    initSteering();
+
+    timer.start();
+    
+    float time;
+
     while(1) {
+        //struct errCounter_t eCounter={0,0,0,0,0,0,0,0};
+
+        //getCurrentErrCount(&eCounter);
+        
+        timer.reset();
 
-        printf("%f\r\n", getVelocity()*3.6f);
+        driveTVD();
+        
+        printf("%f\n\r", 45.0/0xFFFF * calcRequestTorque());
+        
+        //printf("%1.2f\t%1.2f\t%1.6f\t%1.5f\r\n", 45.0f / 0xffff * calcRequestTorque(), getVelocity(), time, getSteerAngle());
+        //printf("%1.6f\r\n", time);
+        time = timer.read();
+
+        //printf("%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t\r\n", eCounter.apsUnderVolt, eCounter.apsExceedVolt, eCounter.apsErrorTolerance, eCounter.apsStick, eCounter.brakeUnderVolt, eCounter.brakeExceedVolt, eCounter.brakeFuzzyVolt, eCounter.brakeOverRide);
+        //printf("take time:%f, apsP:%f, apsS:%f, brake:%f\r", getTakeTime(), 3.3f/65535 * loadRawSensor(APS_PRIMARY), 3.3f/65535 * loadRawSensor(APS_SECONDARY), 3.3f/65535 * loadRawSensor(BRAKE));
+
+        //wait(0.05);
     }
-}
\ No newline at end of file
+}