Revision 1:dc9d55dc362b, committed 2020-01-07

Comitter:

noamnahum

Date:

Tue Jan 07 09:01:18 2020 +0000

Parent:

0:dedba4499e87

Commit message:

TRANARKADI

Changed in this revision

--- a/Map.lib	Tue Dec 31 20:24:17 2019 +0000
+++ b/Map.lib	Tue Jan 07 09:01:18 2020 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/Generic/code/Map/#dad975e2e150
+https://os.mbed.com/users/noamnahum/code/Map/#895b3a247932

--- a/main.cpp	Tue Dec 31 20:24:17 2019 +0000
+++ b/main.cpp	Tue Jan 07 09:01:18 2020 +0000
@@ -1,105 +1,177 @@
-#include "PID.h"
-#include "Motor.h"
+////////////////////////////////////////
+//      Controlling Thorttle          //
+//                                    //
+////////////////////////////////////////
+/* 
+    Pingout:
+    Nucleo-L432KC
+    PA_0 ---> Analogin ---> Read Pedal sensor 1
+    PA_1 ---> Analogin ---> Read Pedal sensor 2
+    CANBUS:
+    PA_11 ---> CANBUS TX
+    PA_12 ----> CANBUS RX
+*/
+
+///////////////
+// Libraries //
+///////////////
 #include <Map.hpp>
 #include "mbed.h"
- //define pins//
+///////////////
+// #defines  //
+///////////////
+
+/////////////
+// Objects //
+/////////////
+
+//Pedal sensor and brake reading
+
 AnalogIn analog_value1(PA_0);
 AnalogIn analog_value2(PA_1);
+AnalogIn analog_value3(PA_3);
+
+//led indicator
+
 DigitalOut myled(LED1);
+
+// can bus
+
 CAN can1(PA_11, PA_12);
-//define variable//
+
+//Serial
+
+Serial pc(USBTX,USBRX);
+
+//Timer
+
+Timer t;
+
+///////////////
+// variables //
+///////////////
+
+int startup = 1;
+
+//Sum counter
 int counter = 0;
+
+// Sending Counter
 int  sendcounter = 0;
+
+//Pedal error counter (0-1)
+
 int errorcounter = 0;
+// Error in pedal
+
 int myerror = 0;
-char datapedal = 0;
-float pedal1, pedal2, sumpedal, subpedal;
-float mypedal,sumPedal = 0;
+
+// pedal value (0-255)
 int pdagree = 0;
-char pedaldata = 0;
-char pedalerror = 1;
-static char message_data[] = {0,0,0}; 
-///////////////////
+
+// brake on or off
+
+int mybrake = 0;
+
+// pedal and brake value calculating variable
+
+float pedal1, pedal2, sumpedal, subpedal, _mybrake;
+float mypedal,sumPedal,sumBrake = 0;
+
+// can message data initial values
+
+char message_data[4] = {0,0,0,0}; 
+
+
 //////////////////
 //define Mapping//
 //////////////////
+
+Map mapvaltovolt = Map(0, 1, 0, 3300); // Maping Volt value from 0-1 to 0-3300
+Map pedaltodagree = Map(900, 1260, 0, 255); // Maping Volt value from 860-1200 to 0 - 255
+
+
 ////////////////////
-Map mapvaltovolt = Map(0, 1, 0, 3300);
-Map pedaltodagree = Map(800, 1220, 0, 255);
-Ticker sensor_sample_timer;
-Ticker data_send;
-Timer t;
-Serial pc(USBTX,USBRX);
-Serial xbee(D1,D0);
-/////////////////////
-////////////////////
+//functions///
 ////////////////////
-//define functions///
-////////////////////
-/////////////////////
-/////////////////////
-void sendData() { // sending Data overcanbus
-    //message_data[0] = pdagree;
-    //message_data[1] = sendcounter;
-    //message_data[2] = myerror;
-    pedaldata = pdagree;
+
+// sending Data overcanbus
+
+void sendData() { 
+    message_data[0] = pdagree;
+    message_data[1] = sendcounter;
+    message_data[2] = myerror;
+    message_data[3] = mybrake;
     //pc.printf("massage about sent %d", pdagree);
-    if(can1.write(CANMessage(1337, &datapedal, 1))) {
-        pc.printf("ok %d", pdagree );
+    if(can1.write(CANMessage(1337, message_data, 3))) {
+        //pc.printf("error %d", message_data[2] );
+        //pc.printf("Data %d\n", message_data[0] );
+        //pc.printf("counter %d\n", message_data[1] );
+        
         sendcounter++;
         if(sendcounter==255){
             sendcounter = 0;
             }
     }
-    t.stop();
 }
-/*void sendError() { //sending Error over Canbus
-        while(1) {
-            if(can1.write(CANMessage(1000, &pedalerror, 1))) {
-                pc.printf("Pedal Error %d", 1);
-                }
-        
-        }
-    }*/
+
 
 void readSensors() { //reading Pedal and Brake sensors
-    t.start();
     pedal1 = analog_value1.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
     pedal2 = analog_value2.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
+    _mybrake = analog_value3.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
     pedal1 = mapvaltovolt.Calculate(pedal1);
     pedal2 = mapvaltovolt.Calculate(pedal2);
+    _mybrake = mapvaltovolt.Calculate(pedal1);
+    
     //pc.printf("pedal1 is: %.4f, matzeret1 is:%.4f\n\r", pedal1, pedal2);
     sumpedal = pedal1+pedal2;
     //pc.printf("sumpedal is: %.4f\n\r", sumpedal);
-    subpedal = abs(3500-sumpedal);
+    subpedal = abs(3400-sumpedal);
     //pc.printf("Subpedal is: %.4f\n\r", subpedal);
     if (subpedal<175) { //checking error
         mypedal = pedaltodagree.Calculate(pedal1);
         sumPedal = sumPedal + mypedal;
-        //setpoint = throttleCmd;
+        sumBrake = sumBrake + _mybrake;
+            if (t.read()>0) {
+                t.stop();
+                t.reset();
+                errorcounter = 0;
+                myerror = 0;
+                }
         //pc.printf("setpedal dagree %.4f\n\r", pdagree);
         counter++;
         if (counter == 10) {
             pdagree = sumPedal/10;
-            datapedal = pdagree;
+            _mybrake = sumBrake/10;
+            if (_mybrake>300) {
+                mybrake = 1;
+                }
+            if (_mybrake>300) {
+                mybrake = 0;
+                }
             sendData();
             counter = 0;
             sumPedal = 0;
-            errorcounter = 0;
+            sumBrake = 0;
         }
-    else {
-        errorcounter++;
-        if (errorcounter > 1000) {
+    }
+    if (subpedal>175) {
+        if (errorcounter == 0) {
+            t.start();
+            errorcounter = 1;
+            }
+        if (t.read() > 0.1) {
+            t.stop();
+            t.reset();
             myerror = 1;
+            sendData();
             }
-        
-        } 
-        
-    }
+        }
 }
 int main() {
-    //can1.frequency(512000);
-    sensor_sample_timer.attach(readSensors, 0.0001);
-    //xbee.baud(115200);
-    //pc.baud(115200);
+    while(1) {
+        readSensors();
+        }
+
 }