Diff: main.cpp

Revision:

3:68d91895d991

Parent:

2:7e4930f48021

Child:

4:66b2a94a607f

--- a/main.cpp	Thu Mar 07 21:05:08 2013 +0000
+++ b/main.cpp	Fri Mar 08 18:58:02 2013 +0000
@@ -22,6 +22,7 @@
 float steering,steering2;                                                         //input steering wheel angle in degrees
 float throttle, throttle2;                                                         //throttle input from pedal
 float yaw_rate;
+float brake, brake2;
 
 // - - - Outputs - - - //
 
@@ -43,11 +44,6 @@
 float turnradius;
 float wratio_desired;                                                   //Right/Left
 
-//Openloop Flag
-volatile bool openloop_lowrpm = true;
-volatile bool openloop_driver = false;
-const float rpm_openloop_limit = 100;
-
 //PID constants
 float kp=0;
 float kd=0;
@@ -71,17 +67,14 @@
 float rpm_FrontRight;
 float rpm_FrontLeft;
 
-// - - - - FMEA OUTPUT - - - - //
-bool shutdown = false;
-volatile int fmea_switch = 0;
-Ticker for_FmeaCall;
-void FmeaCall();
-
 //////////////////////////////////////CAN DATA TO DAQ////////////////////////////////////////////
 CAN CANtoDAQ(p9, p10);  //Check PIN Numbers;
 Ticker SendDataToDAQ;
 char CANdataSet1[8];
 char CANdataSet2[8];
+char CANdataSet3[8];
+uint8_t FMEAHighByte = 0x00;
+uint8_t FMEALowByte = 0x00;
 
 // add motor temperatures and motor currents
 void PopulateData(void)
@@ -103,6 +96,15 @@
     CANdataSet2[5] = (char)(rpm_FrontRight/1000 * 255);
     CANdataSet2[6] = (char)(yaw_rate/5 * 255);                         //Decide range later. For now 0 to 5.
     CANdataSet2[7] = 0;    
+    
+    CANdataSet3[0] = (char)(currentLeftMux>>8);
+    CANdataSet3[1] = (char)(currentRightMux>>8);
+    CANdataSet3[2] = (char)(tempLeftMux>>8);
+    CANdataSet3[3] = (char)(tempRightMux>>8);
+    CANdataSet3[4] = (char)(FMEAHighByte);//FMEA HIGH BYTE
+    CANdataSet3[5] = (char)(FMEALowByte);//FMEA LOW BYTE
+    CANdataSet3[6] = (char)(yaw_rate/5 * 255);                         //Decide range later. For now 0 to 5.
+    CANdataSet3[7] = 0;        
 }
 
 void SendData(void)
@@ -110,6 +112,7 @@
     PopulateData();
     CANtoDAQ.write(CANMessage(1000, CANdataSet1, 8));   
     CANtoDAQ.write(CANMessage(1001, CANdataSet2, 8));
+    CANtoDAQ.write(CANMessage(1002, CANdataSet3, 8));
 }
 
 ///////////////////////////////RECEIVE SPI DATA FROM ICAP//////////////////////////////////////////
@@ -118,6 +121,7 @@
 #define SPI_BYTE_ARRAY_SIZE 13
 uint8_t SPIDataFromICAP[SPI_BYTE_ARRAY_SIZE];
 uint8_t startByte, endByte;
+uint8_t flagsFromICAP;
 int SPIByteCounter = 0;
 bool SPIError = false;
 Ticker PullDataFromICAP;
@@ -129,22 +133,23 @@
     if (((char)startByte != '@') || ((char)endByte != '#'))
     {
         //Some Problem with SPI.
+        shutdown = true;
         SPIError = true;
+        FMEALowByte|= 0x20; //0b0010 0000
+        FMEAHighByte|= 0x04; //0b0000 0100    
     }
     
-    /*    
-    steering = SPIDataFromICAP[1];
-    steering2 = SPIDataFromICAP[2];
-    throttle = SPIDataFromICAP[3];
-    throttle2 = SPIDataFromICAP[4];
-    brake = SPIDataFromICAP[5];
-    brake2 = SPIDataFromICAP[6];
-    rpm_Left = SPIDataFromICAP[7];
-    rpm_Right = SPIDataFromICAP[8];
-    rpm_FrontLeft = SPIDataFromICAP[9];
-    rpm_FrontRight = SPIDataFromICAP[10];
-    flagsFromICAP = SPIDataFromICAP[11];    
-    */    
+    steering = -110 + SPIDataFromICAP[1]*220/255;
+    steering2 = -110 + SPIDataFromICAP[2]*220/255;
+    throttle = SPIDataFromICAP[3]*5/255;
+    throttle2 = SPIDataFromICAP[4]*5/255;
+    brake = SPIDataFromICAP[5];                                                 //Abhi ke liye let be be 0 to 255 only                                 
+    brake2 = SPIDataFromICAP[6];                                                //Abhi ke liye let be be 0 to 255 only
+    rpm_Left = SPIDataFromICAP[7]*1000/255;
+    rpm_Right = SPIDataFromICAP[8]*1000/255;
+    rpm_FrontLeft = SPIDataFromICAP[9]*1000/255;
+    rpm_FrontRight = SPIDataFromICAP[10]*1000/255;
+    flagsFromICAP = SPIDataFromICAP[11];        
 }
 
 void SPIPullData()
@@ -228,50 +233,38 @@
     }
 }
 
-////////////////////////////////////////////////////MAIN////////////////////////////////////////////////////
-int main()
-{   
-    for_FmeaCall.attach(&FmeaCall,1);
-    for_algo.attach_us(&ediff_func,(algoperiod));                   //call ediff func every "algoperiod" sec  
-    SendDataToDAQ.attach(&SendData, 0.05);
-    
-    //DAC in Main
-    initDAC();
-    for_DAC.attach(&outputToDAC, 0.01);    
-    
-    //MUX in Main
-    for_Mux.attach(&MuxCall, 0.5);
-    
-    cs = 1;
-    spi.format(8,3);
-    spi.frequency(1000000);
-    PullDataFromICAP.attach(&SPIPullData, 0.02);
-    spi.write(0xAA);    
+
+// - - - - FMEA OUTPUT - - - - //
+
+//Openloop Flag
+volatile bool openloop_lowrpm = true;
+volatile bool openloop_driver = false;
+const float rpm_openloop_limit = 100;
+
 
-    while(1)
-    {
-        //Take steering 1,2 and throttle 1,2 from Input capture
-    }    
-}
+bool Dash = false;
+bool throttleLeftDACFailure = false;
+bool throttleRightDACFailure = false;
+bool throttleComparisonFailure = false;
+bool steeringComparisonFailure = false;
+bool useBoschSteeringRequest = false;
+bool useBoschSteeringDriverApproval = false;
+bool shutdown = false;
+bool throttleLeftlimitcross = false;
+bool throttleRightlimitcross = false;
 
-//Function reads stearing and throttle input, calls PID Function and gives Throttle Left and Right PWM outputs
-void ediff_func()    
-{        
-    //call pid function which gives v1, v2 output    
-    pid();                  
-    
-    //send throttle_Left and throttle_Right to Kelly Controllers 
-}
+volatile int fmea_switch = 0;
+Ticker for_FmeaCall;
 
 void FmeaCall()
 {
     switch (fmea_switch)
     {
         case 0: 
-        throttle_Left_pulldown_fmea();
+        throttle_Left_limitcross_fmea();
         break;
         case 1: 
-        throttle_Right_pulldown_fmea();
+        throttle_Right_limitcross_fmea();
         break;
         case 2: 
         throttle_comparison_fmea();
@@ -292,3 +285,52 @@
         fmea_switch = 0;
     }    
 }
+
+////////////////////////////////////////////////////MAIN////////////////////////////////////////////////////
+int main()
+{   
+    for_FmeaCall.attach(&FmeaCall,1);
+    for_algo.attach_us(&ediff_func,(algoperiod));                   //call ediff func every "algoperiod" sec  
+    SendDataToDAQ.attach(&SendData, 0.05);
+        
+    //DAC in Main
+    initDAC();
+    for_DAC.attach(&outputToDAC, 0.01);    
+    
+    //MUX in Main
+    for_Mux.attach(&MuxCall, 0.5);
+    
+    cs = 1;
+    spi.format(8,3);
+    spi.frequency(1000000);
+    PullDataFromICAP.attach(&SPIPullData, 0.02);
+    spi.write(0xAA);    
+
+    while(1)
+    {
+        //openloop_driver is set if driver gives approval to dash message request for openloop
+        
+        // useBoschSteeringDriverApproval is set if driver gives approval to dash message request for using Bosch Steering
+        
+        if (Dash == true)
+        {
+            if (flagsFromICAP & 0x78) //0b 0111 1000
+            {
+                //send message to DASH that asks driver permission for open loop
+            }
+            if (useBoschSteeringRequest)
+            {
+                //send message to DASH that asks driver permission for using Bosch Steering
+            }  
+        }
+    }    
+}
+
+//Function reads stearing and throttle input, calls PID Function and gives Throttle Left and Right PWM outputs
+void ediff_func()    
+{        
+    //call pid function which gives v1, v2 output    
+    pid();                  
+    
+    //send throttle_Left and throttle_Right to Kelly Controllers 
+}