Diff: recive.cpp

Revision:

1:f0e37d1d1452

Parent:

0:d6eafce8e991

Child:

2:0d44b696736b

--- a/recive.cpp	Fri Dec 27 17:50:14 2019 +0000
+++ b/recive.cpp	Tue Jan 07 09:03:33 2020 +0000
@@ -1,17 +1,230 @@
-#include "mbed.h"
+////////////////////////////////////////
+//      Controlling Thorttle          //
+//                                    //
+////////////////////////////////////////
+/* 
+    Pingout:
+    Nucleo-L432KC
+    PA_0 ---> Analogin ---> Read Potentiometer 1
+    PA_1 ---> Analogin ---> Read Potentiometer 2
+    PA_3 ----> DigitalOut ---> to Led MOSFET
+    LED1 ----> DigitalOut ----> Error Indication
+    CANBUS:
+    PA_11 ---> CANBUS TX
+    PA_12 ----> CANBUS RX
+    Motor:
+    PA_8 (D9) --- PWMOUT
+    PA_5 ---> Digitalout ---> Motor FWD direction
+    PA_6 ---> Digitalout ---> Motor RWD direction
+*/
 
-DigitalOut led1(LED1);
+///////////////
+// Libraries //
+///////////////
+    
+#include "PID.h"
+#include "Motor.h"
+#include <Map.hpp>
+#include "mbed.h"
+///////////////
+// #defines  //
+///////////////
+
+/////////////
+// Objects //
+/////////////
+
+//thorttle potentiometer
+
+AnalogIn analog_value1(PA_0);
+AnalogIn analog_value2(PA_1);
+
+//led indicator
+
+DigitalOut myled(LED1);
+
+// brake signalout
+
+DigitalOut mybrake(PA_3);
+
+// can bus
+
 CAN can1(PA_11, PA_12);
 
-int main() {
-    printf("main()\n");
+//Motor Control
+
+Motor myMotor(PA_8, PA_5, PA_6);
+
+//Brake signal 
+
+//Serial
+
+Serial pc(USBTX,USBRX);
+
+//Timer
+
+Timer t;
+Timer can;
+
+///////////////
+// variables //
+///////////////
+int startup = 0;
+int counter = 0;
+int errorcounter = 0;
+
+// PID
+
+// pid output limits
+
+const float output_lower_limit = -255;          
+const float output_upper_limit = 255;
+
+//pid constants
+
+const float kp = 1.2;
+const float ki = 0.5;
+const float kd = 3;
+
+//Time sample
+
+const float Ts = 0.001;
+
+//Thorttle and pedal intital values
+float  thorttle1, thorttle2, sumThorttle, subThorttle; 
+float mdagree, pdagree = 0;
+float mythorttle,sumthorttle = 0;
+
+//Motor speed pwm -1<speed<1
+
+float speed = 0;
+
+// Define PID object //
+
+PID pid(&pdagree, &mdagree, &speed, output_lower_limit, output_upper_limit,kp, ki, kd, Ts);
+
+//////////////////
+//define Mapping//
+//////////////////
+
+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 nspeed = Map(-255, 255, -1, 1); // Maping out put speed from -255-255 to -1-1
+
+
+////////////////////
+//define functions///
+////////////////////
+
+// After pid calculate and Sensor data recieve this function command the output to the motor
+
+void BrakeOn() {
+    mybrake = 1;
+    }
+void BrakeOff() {
+    mybrake = 0;
+    }
+void commandMotor(){ 
+    float Motorcommand = nspeed.Calculate(speed);
+    myMotor.speed(Motorcommand);
+}  
+
+// if there is Error this function Turn the Motor speed to 0 // Close the Thorttle and then The engine is off
+void sendError() { 
+    myMotor.speed(0);
+    //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;
-    while(1) {
         if(can1.read(msg)) {
-            printf("got it\n\r");
-            printf("Message received: %d,,\n\r", msg.data[0]);
-            led1 = !led1;
-        } 
-        wait(0.2);
-    }
+            if (can.read() > 0) {
+                can.stop();
+                can.reset();
+                }
+            //pc.printf("%d", msg.data[0]);
+            if (msg.data[2]==1){
+                sendError();
+                }
+            if (msg.data[3]==1){
+                BrakeOn();
+                }
+            if (msg.data[3]==0){
+                BrakeOff();
+                }
+            if(msg.data[0]<30){
+                pdagree = 30;
+                }
+            if(msg.data[0]>30){
+                pdagree = msg.data[0]*1.5;
+                }
+            //pc.printf("Subthorttle is: %.4f\n\r", pdagree);
+            //pc.printf("Error is: %.4f\n\r", msg.data[2]);
+            }
+        if (!can1.read(msg)) {
+            can.start();
+            }
+        if (can.read() > 0.1) { // if message not recieved more then 0.1 sec send error
+            can.stop();
+            can.reset();
+            sendError();
+            }
+        }
+                
+//reading Thorttle sensors
+
+void readSensors() { 
+    thorttle1 = analog_value1.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
+    thorttle2 = analog_value2.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
+    thorttle1 = mapvaltovolt.Calculate(thorttle1);
+    thorttle2 = mapvaltovolt.Calculate(thorttle2);
+    sumThorttle = thorttle1+thorttle2;
+    subThorttle = abs(3500-sumThorttle);
+    //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
+        mythorttle = mtodagree.Calculate(thorttle1);
+        sumthorttle = sumthorttle + mythorttle;
+        counter++;
+        if (t.read()>0) {
+            errorcounter = 0;
+            t.stop();
+            t.reset();
+            }
+        if (counter == 10) {
+            mdagree = sumthorttle/10;
+            getData();
+            counter = 0;
+            sumthorttle = 0;
+            }
+        }
+    if (subThorttle>350) {
+            if (errorcounter == 0) {
+                t.start();
+                errorcounter = 1;
+                }
+            if (t.read() > 0.1) {
+                t.stop();
+                t.reset();
+                sendError();
+                }
+            }
+}
+int main() {
+    pid.start();
+    while(1){
+        readSensors();
+        pid.sample();
+        commandMotor();
+        //wait(Ts);
+        }
 }
\ No newline at end of file