Diff: recive.cpp

Revision:

2:0d44b696736b

Parent:

1:f0e37d1d1452

Child:

3:94eac3bdfbf5

--- a/recive.cpp	Tue Jan 07 09:03:33 2020 +0000
+++ b/recive.cpp	Thu Mar 05 16:49:12 2020 +0000
@@ -38,7 +38,7 @@
 
 AnalogIn analog_value1(PA_0);
 AnalogIn analog_value2(PA_1);
-
+int pid_event = 0;
 //led indicator
 
 DigitalOut myled(LED1);
@@ -65,6 +65,7 @@
 
 Timer t;
 Timer can;
+Timer tpid;
 
 ///////////////
 // variables //
@@ -82,14 +83,26 @@
 
 //pid constants
 
-const float kp = 1.2;
-const float ki = 0.5;
-const float kd = 3;
+const float kp = 1.5;
+const float ki = 2;
+const float kd = 0;
 
 //Time sample
 
 const float Ts = 0.001;
 
+//Tickers
+Ticker main_ticker;
+Ticker motor;
+
+//Flags for indication if something happend - error handling
+int pidflag = 0;
+int canflag = 0;
+int cantimeoutflag = 0; // if can message is not recived more then 100ms
+int rsflag = 0;
+int terrorflag = 0;
+int perrorflag = 0;
+
 //Thorttle and pedal intital values
 float  thorttle1, thorttle2, sumThorttle, subThorttle; 
 float mdagree, pdagree = 0;
@@ -108,7 +121,7 @@
 //////////////////
 
 Map mapvaltovolt = Map(0, 1, 0, 3300); // Maping Volt value from 0-1 to 0-3300
-Map mtodagree = Map(490, 3150, 0, 255); // Maping Volt value from 490-3150 to 0 - 255
+Map mtodagree = Map(230, 3070, 0, 255); // Maping Volt value from 490-3150 to 0 - 255
 Map nspeed = Map(-255, 255, -1, 1); // Maping out put speed from -255-255 to -1-1
 
 
@@ -131,29 +144,33 @@
 
 // if there is Error this function Turn the Motor speed to 0 // Close the Thorttle and then The engine is off
 void sendError() { 
+    main_ticker.detach();
     myMotor.speed(0);
-    //pid.stop();
-    while (1) {
+    pid.stop();
+}
+    /*while (1) {
         BrakeOn();
         //pc.printf("Error is: %.4f\n\r", 1);
         wait(1);
         BrakeOff();
         wait(1);
         }
-    }
+    }*/
     
 // get Data overcanbus    
 void getData() {
     //pc.printf("%d", 1); 
     CANMessage msg;
         if(can1.read(msg)) {
+            //pc.printf("Pedal sensor worng values\n");
+            pc.printf("my data: %d", msg.data[0]);
             if (can.read() > 0) {
                 can.stop();
                 can.reset();
                 }
-            //pc.printf("%d", msg.data[0]);
             if (msg.data[2]==1){
-                sendError();
+                perrorflag = 1;
+                //sendError();
                 }
             if (msg.data[3]==1){
                 BrakeOn();
@@ -165,8 +182,9 @@
                 pdagree = 30;
                 }
             if(msg.data[0]>30){
-                pdagree = msg.data[0]*1.5;
+                pdagree = msg.data[0];
                 }
+            //pid.sample();
             //pc.printf("Subthorttle is: %.4f\n\r", pdagree);
             //pc.printf("Error is: %.4f\n\r", msg.data[2]);
             }
@@ -176,9 +194,11 @@
         if (can.read() > 0.1) { // if message not recieved more then 0.1 sec send error
             can.stop();
             can.reset();
-            sendError();
+            cantimeoutflag = 1;
+            //sendError();
             }
         }
+        
                 
 //reading Thorttle sensors
 
@@ -189,9 +209,12 @@
     thorttle2 = mapvaltovolt.Calculate(thorttle2);
     sumThorttle = thorttle1+thorttle2;
     subThorttle = abs(3500-sumThorttle);
+    getData();
+    pid.sample();
+    commandMotor();
     //pc.printf("matzeret1 is: %.4f, matzeret2 is:%.4f\n\r", thorttle1, thorttle2);
     //pc.printf("Subthorttle is: %.4f\n\r", subThorttle);
-    if (subThorttle<350) { //checking error
+    if (subThorttle<600) { //checking error
         mythorttle = mtodagree.Calculate(thorttle1);
         sumthorttle = sumthorttle + mythorttle;
         counter++;
@@ -202,12 +225,12 @@
             }
         if (counter == 10) {
             mdagree = sumthorttle/10;
-            getData();
+            //getData();
             counter = 0;
             sumthorttle = 0;
             }
         }
-    if (subThorttle>350) {
+    if (subThorttle>600) {
             if (errorcounter == 0) {
                 t.start();
                 errorcounter = 1;
@@ -215,16 +238,34 @@
             if (t.read() > 0.1) {
                 t.stop();
                 t.reset();
+                terrorflag = 1;
                 sendError();
                 }
             }
 }
 int main() {
+    pc.baud(9600);
     pid.start();
+    //tpid.start();
+    main_ticker.attach(readSensors, 0.0005);
+    //motor.attach(commandMotor, 0.0005);
     while(1){
-        readSensors();
-        pid.sample();
-        commandMotor();
+        /*if (cantimeoutflag = 1){
+            pc.printf(pc.printf("Canbus message timeout\n"););
+            }*/
+        /*if (perrorflag = 1){
+            myMotor.speed(0);
+            pc.printf("Pedal sensor worng values\n");
+            }*/
+        /*if (merrorflag = 1){
+            pc.printf("Pedal sensor worng values\n");
+            pc.printf("matzeret1 is: %.4f, matzeret2 is:%.4f\n\r", thorttle1, thorttle2);
+            }*/
+        /*if (merrorflag = 1){
+            pc.printf("Pedal sensor worng values\n");
+            }*/
+        //pc.printf("%.4f,%.4f,%.4f\n", tpid.read(), pdagree, mdagree);
+        
         //wait(Ts);
         }
 }
\ No newline at end of file