Noam Nahum / Mbed 2 deprecated CANrecive

FORARKADI

Dependencies:   mbed mypidror1 Motor Map

recive.cpp@6:13f47e31348d, 2020-03-13 (annotated)

Committer:
noamnahum
Date:
Fri Mar 13 12:23:35 2020 +0000
Revision:
6:13f47e31348d
Parent:
5:8ef69c6f3718
Child:
7:34118a0098e8
CAN receive with 1 sensor worng;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
noamnahum 1:f0e37d1d1452 1 ////////////////////////////////////////
noamnahum 1:f0e37d1d1452 2 // Controlling Thorttle //
noamnahum 1:f0e37d1d1452 3 // //
noamnahum 1:f0e37d1d1452 4 ////////////////////////////////////////
noamnahum 1:f0e37d1d1452 5 /*
noamnahum 1:f0e37d1d1452 6 Pingout:
noamnahum 1:f0e37d1d1452 7 Nucleo-L432KC
noamnahum 1:f0e37d1d1452 8 PA_0 ---> Analogin ---> Read Potentiometer 1
noamnahum 1:f0e37d1d1452 9 PA_1 ---> Analogin ---> Read Potentiometer 2
noamnahum 1:f0e37d1d1452 10 PA_3 ----> DigitalOut ---> to Led MOSFET
noamnahum 1:f0e37d1d1452 11 LED1 ----> DigitalOut ----> Error Indication
noamnahum 1:f0e37d1d1452 12 CANBUS:
noamnahum 1:f0e37d1d1452 13 PA_11 ---> CANBUS TX
noamnahum 1:f0e37d1d1452 14 PA_12 ----> CANBUS RX
noamnahum 1:f0e37d1d1452 15 Motor:
noamnahum 1:f0e37d1d1452 16 PA_8 (D9) --- PWMOUT
noamnahum 1:f0e37d1d1452 17 PA_5 ---> Digitalout ---> Motor FWD direction
noamnahum 1:f0e37d1d1452 18 PA_6 ---> Digitalout ---> Motor RWD direction
noamnahum 1:f0e37d1d1452 19 */
drorbalbul 0:d6eafce8e991 20
noamnahum 1:f0e37d1d1452 21 ///////////////
noamnahum 1:f0e37d1d1452 22 // Libraries //
noamnahum 1:f0e37d1d1452 23 ///////////////
noamnahum 1:f0e37d1d1452 24
noamnahum 1:f0e37d1d1452 25 #include "PID.h"
noamnahum 1:f0e37d1d1452 26 #include "Motor.h"
noamnahum 1:f0e37d1d1452 27 #include <Map.hpp>
noamnahum 1:f0e37d1d1452 28 #include "mbed.h"
noamnahum 1:f0e37d1d1452 29 ///////////////
noamnahum 1:f0e37d1d1452 30 // #defines //
noamnahum 1:f0e37d1d1452 31 ///////////////
noamnahum 1:f0e37d1d1452 32
noamnahum 1:f0e37d1d1452 33 /////////////
noamnahum 1:f0e37d1d1452 34 // Objects //
noamnahum 1:f0e37d1d1452 35 /////////////
noamnahum 1:f0e37d1d1452 36
noamnahum 1:f0e37d1d1452 37 //thorttle potentiometer
noamnahum 1:f0e37d1d1452 38
noamnahum 1:f0e37d1d1452 39 AnalogIn analog_value1(PA_0);
noamnahum 1:f0e37d1d1452 40 AnalogIn analog_value2(PA_1);
noamnahum 2:0d44b696736b 41 int pid_event = 0;
noamnahum 1:f0e37d1d1452 42 //led indicator
noamnahum 1:f0e37d1d1452 43
noamnahum 1:f0e37d1d1452 44 DigitalOut myled(LED1);
noamnahum 1:f0e37d1d1452 45
noamnahum 1:f0e37d1d1452 46 // brake signalout
noamnahum 1:f0e37d1d1452 47
noamnahum 1:f0e37d1d1452 48 DigitalOut mybrake(PA_3);
noamnahum 1:f0e37d1d1452 49
noamnahum 1:f0e37d1d1452 50 // can bus
noamnahum 1:f0e37d1d1452 51
drorbalbul 0:d6eafce8e991 52 CAN can1(PA_11, PA_12);
drorbalbul 0:d6eafce8e991 53
noamnahum 1:f0e37d1d1452 54 //Motor Control
noamnahum 1:f0e37d1d1452 55
noamnahum 1:f0e37d1d1452 56 Motor myMotor(PA_8, PA_5, PA_6);
noamnahum 1:f0e37d1d1452 57
noamnahum 1:f0e37d1d1452 58 //Brake signal
noamnahum 1:f0e37d1d1452 59
noamnahum 1:f0e37d1d1452 60 //Serial
noamnahum 1:f0e37d1d1452 61
noamnahum 1:f0e37d1d1452 62 Serial pc(USBTX,USBRX);
noamnahum 1:f0e37d1d1452 63
noamnahum 1:f0e37d1d1452 64 //Timer
noamnahum 1:f0e37d1d1452 65
noamnahum 1:f0e37d1d1452 66 Timer t;
noamnahum 1:f0e37d1d1452 67 Timer can;
noamnahum 2:0d44b696736b 68 Timer tpid;
noamnahum 1:f0e37d1d1452 69
noamnahum 1:f0e37d1d1452 70 ///////////////
noamnahum 1:f0e37d1d1452 71 // variables //
noamnahum 1:f0e37d1d1452 72 ///////////////
noamnahum 1:f0e37d1d1452 73 int startup = 0;
noamnahum 1:f0e37d1d1452 74 int counter = 0;
noamnahum 1:f0e37d1d1452 75 int errorcounter = 0;
noamnahum 3:94eac3bdfbf5 76 int recievecounter = 0;
noamnahum 1:f0e37d1d1452 77
noamnahum 1:f0e37d1d1452 78 // PID
noamnahum 1:f0e37d1d1452 79
noamnahum 1:f0e37d1d1452 80 // pid output limits
noamnahum 1:f0e37d1d1452 81
noamnahum 1:f0e37d1d1452 82 const float output_lower_limit = -255;
noamnahum 1:f0e37d1d1452 83 const float output_upper_limit = 255;
noamnahum 1:f0e37d1d1452 84
noamnahum 1:f0e37d1d1452 85 //pid constants
noamnahum 1:f0e37d1d1452 86
noamnahum 2:0d44b696736b 87 const float kp = 1.5;
noamnahum 5:8ef69c6f3718 88 const float ki = 3;
noamnahum 2:0d44b696736b 89 const float kd = 0;
noamnahum 1:f0e37d1d1452 90
noamnahum 1:f0e37d1d1452 91 //Time sample
noamnahum 1:f0e37d1d1452 92
noamnahum 1:f0e37d1d1452 93 const float Ts = 0.001;
noamnahum 1:f0e37d1d1452 94
noamnahum 2:0d44b696736b 95 //Tickers
noamnahum 2:0d44b696736b 96 Ticker main_ticker;
noamnahum 2:0d44b696736b 97 Ticker motor;
noamnahum 3:94eac3bdfbf5 98 Ticker syserror;
noamnahum 2:0d44b696736b 99
noamnahum 2:0d44b696736b 100 //Flags for indication if something happend - error handling
noamnahum 2:0d44b696736b 101 int pidflag = 0;
noamnahum 2:0d44b696736b 102 int canflag = 0;
noamnahum 2:0d44b696736b 103 int cantimeoutflag = 0; // if can message is not recived more then 100ms
noamnahum 2:0d44b696736b 104 int rsflag = 0;
noamnahum 2:0d44b696736b 105 int terrorflag = 0;
noamnahum 2:0d44b696736b 106 int perrorflag = 0;
noamnahum 2:0d44b696736b 107
noamnahum 1:f0e37d1d1452 108 //Thorttle and pedal intital values
noamnahum 1:f0e37d1d1452 109 float thorttle1, thorttle2, sumThorttle, subThorttle;
noamnahum 1:f0e37d1d1452 110 float mdagree, pdagree = 0;
noamnahum 1:f0e37d1d1452 111 float mythorttle,sumthorttle = 0;
noamnahum 1:f0e37d1d1452 112
noamnahum 1:f0e37d1d1452 113 //Motor speed pwm -1<speed<1
noamnahum 1:f0e37d1d1452 114
noamnahum 1:f0e37d1d1452 115 float speed = 0;
noamnahum 1:f0e37d1d1452 116
noamnahum 1:f0e37d1d1452 117 // Define PID object //
noamnahum 1:f0e37d1d1452 118
noamnahum 1:f0e37d1d1452 119 PID pid(&pdagree, &mdagree, &speed, output_lower_limit, output_upper_limit,kp, ki, kd, Ts);
noamnahum 1:f0e37d1d1452 120
noamnahum 1:f0e37d1d1452 121 //////////////////
noamnahum 1:f0e37d1d1452 122 //define Mapping//
noamnahum 1:f0e37d1d1452 123 //////////////////
noamnahum 1:f0e37d1d1452 124
noamnahum 1:f0e37d1d1452 125 Map mapvaltovolt = Map(0, 1, 0, 3300); // Maping Volt value from 0-1 to 0-3300
noamnahum 2:0d44b696736b 126 Map mtodagree = Map(230, 3070, 0, 255); // Maping Volt value from 490-3150 to 0 - 255
noamnahum 1:f0e37d1d1452 127 Map nspeed = Map(-255, 255, -1, 1); // Maping out put speed from -255-255 to -1-1
noamnahum 1:f0e37d1d1452 128
noamnahum 1:f0e37d1d1452 129
noamnahum 1:f0e37d1d1452 130 ////////////////////
noamnahum 1:f0e37d1d1452 131 //define functions///
noamnahum 1:f0e37d1d1452 132 ////////////////////
noamnahum 1:f0e37d1d1452 133
noamnahum 1:f0e37d1d1452 134 // After pid calculate and Sensor data recieve this function command the output to the motor
noamnahum 1:f0e37d1d1452 135
noamnahum 1:f0e37d1d1452 136 void BrakeOn() {
noamnahum 1:f0e37d1d1452 137 mybrake = 1;
noamnahum 1:f0e37d1d1452 138 }
noamnahum 1:f0e37d1d1452 139 void BrakeOff() {
noamnahum 1:f0e37d1d1452 140 mybrake = 0;
noamnahum 1:f0e37d1d1452 141 }
noamnahum 1:f0e37d1d1452 142 void commandMotor(){
noamnahum 1:f0e37d1d1452 143 float Motorcommand = nspeed.Calculate(speed);
noamnahum 1:f0e37d1d1452 144 myMotor.speed(Motorcommand);
noamnahum 1:f0e37d1d1452 145 }
noamnahum 1:f0e37d1d1452 146
noamnahum 1:f0e37d1d1452 147 // if there is Error this function Turn the Motor speed to 0 // Close the Thorttle and then The engine is off
noamnahum 1:f0e37d1d1452 148 void sendError() {
noamnahum 3:94eac3bdfbf5 149 if (errorcounter == 1){
noamnahum 3:94eac3bdfbf5 150 main_ticker.detach();
noamnahum 3:94eac3bdfbf5 151 pid.stop();
noamnahum 3:94eac3bdfbf5 152 syserror.attach(sendError, 0.0005);
noamnahum 3:94eac3bdfbf5 153 }
noamnahum 1:f0e37d1d1452 154 myMotor.speed(0);
noamnahum 2:0d44b696736b 155 }
noamnahum 2:0d44b696736b 156 /*while (1) {
noamnahum 1:f0e37d1d1452 157 BrakeOn();
noamnahum 1:f0e37d1d1452 158 //pc.printf("Error is: %.4f\n\r", 1);
noamnahum 1:f0e37d1d1452 159 wait(1);
noamnahum 1:f0e37d1d1452 160 BrakeOff();
noamnahum 1:f0e37d1d1452 161 wait(1);
noamnahum 1:f0e37d1d1452 162 }
noamnahum 2:0d44b696736b 163 }*/
noamnahum 1:f0e37d1d1452 164
noamnahum 1:f0e37d1d1452 165 // get Data overcanbus
noamnahum 1:f0e37d1d1452 166 void getData() {
noamnahum 1:f0e37d1d1452 167 //pc.printf("%d", 1);
drorbalbul 0:d6eafce8e991 168 CANMessage msg;
drorbalbul 0:d6eafce8e991 169 if(can1.read(msg)) {
noamnahum 3:94eac3bdfbf5 170 recievecounter ++;
noamnahum 2:0d44b696736b 171 //pc.printf("Pedal sensor worng values\n");
noamnahum 3:94eac3bdfbf5 172 //pc.printf("my data: %d", msg.data[2]);
noamnahum 1:f0e37d1d1452 173 if (can.read() > 0) {
noamnahum 1:f0e37d1d1452 174 can.stop();
noamnahum 1:f0e37d1d1452 175 can.reset();
noamnahum 1:f0e37d1d1452 176 }
noamnahum 1:f0e37d1d1452 177 if (msg.data[2]==1){
noamnahum 3:94eac3bdfbf5 178 errorcounter = 1;
noamnahum 2:0d44b696736b 179 perrorflag = 1;
noamnahum 4:d40399d8ae44 180 sendError();
noamnahum 1:f0e37d1d1452 181 }
noamnahum 3:94eac3bdfbf5 182 if (msg.data[2]==0){
noamnahum 3:94eac3bdfbf5 183 perrorflag = 0;
noamnahum 3:94eac3bdfbf5 184 }
noamnahum 1:f0e37d1d1452 185 if (msg.data[3]==1){
noamnahum 1:f0e37d1d1452 186 BrakeOn();
noamnahum 1:f0e37d1d1452 187 }
noamnahum 1:f0e37d1d1452 188 if (msg.data[3]==0){
noamnahum 1:f0e37d1d1452 189 BrakeOff();
noamnahum 1:f0e37d1d1452 190 }
noamnahum 3:94eac3bdfbf5 191 pdagree = msg.data[0];
noamnahum 1:f0e37d1d1452 192 }
noamnahum 1:f0e37d1d1452 193 if (!can1.read(msg)) {
noamnahum 1:f0e37d1d1452 194 can.start();
noamnahum 1:f0e37d1d1452 195 }
noamnahum 1:f0e37d1d1452 196 if (can.read() > 0.1) { // if message not recieved more then 0.1 sec send error
noamnahum 1:f0e37d1d1452 197 can.stop();
noamnahum 3:94eac3bdfbf5 198 //can.reset();
noamnahum 4:d40399d8ae44 199 errorcounter = 1;
noamnahum 2:0d44b696736b 200 cantimeoutflag = 1;
noamnahum 4:d40399d8ae44 201 sendError();
noamnahum 1:f0e37d1d1452 202 }
noamnahum 1:f0e37d1d1452 203 }
noamnahum 2:0d44b696736b 204
noamnahum 1:f0e37d1d1452 205
noamnahum 1:f0e37d1d1452 206 //reading Thorttle sensors
noamnahum 1:f0e37d1d1452 207
noamnahum 1:f0e37d1d1452 208 void readSensors() {
noamnahum 1:f0e37d1d1452 209 thorttle1 = analog_value1.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
noamnahum 1:f0e37d1d1452 210 thorttle2 = analog_value2.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
noamnahum 1:f0e37d1d1452 211 thorttle1 = mapvaltovolt.Calculate(thorttle1);
noamnahum 1:f0e37d1d1452 212 thorttle2 = mapvaltovolt.Calculate(thorttle2);
noamnahum 1:f0e37d1d1452 213 sumThorttle = thorttle1+thorttle2;
noamnahum 3:94eac3bdfbf5 214 subThorttle = abs(3300-sumThorttle);
noamnahum 2:0d44b696736b 215 getData();
noamnahum 2:0d44b696736b 216 pid.sample();
noamnahum 2:0d44b696736b 217 commandMotor();
noamnahum 1:f0e37d1d1452 218 //pc.printf("matzeret1 is: %.4f, matzeret2 is:%.4f\n\r", thorttle1, thorttle2);
noamnahum 1:f0e37d1d1452 219 //pc.printf("Subthorttle is: %.4f\n\r", subThorttle);
noamnahum 6:13f47e31348d 220 //checking error
noamnahum 6:13f47e31348d 221 mythorttle = mtodagree.Calculate(thorttle1);
noamnahum 6:13f47e31348d 222 sumthorttle = sumthorttle + mythorttle;
noamnahum 6:13f47e31348d 223 counter++;
noamnahum 6:13f47e31348d 224 if (t.read()>0) {
noamnahum 6:13f47e31348d 225 errorcounter = 0;
noamnahum 6:13f47e31348d 226 t.stop();
noamnahum 6:13f47e31348d 227 t.reset();
noamnahum 1:f0e37d1d1452 228 }
noamnahum 6:13f47e31348d 229 if (counter == 10) {
noamnahum 6:13f47e31348d 230 mdagree = sumthorttle/10;
noamnahum 6:13f47e31348d 231 counter = 0;
noamnahum 6:13f47e31348d 232 sumthorttle = 0;
noamnahum 6:13f47e31348d 233 }
noamnahum 6:13f47e31348d 234
noamnahum 6:13f47e31348d 235 /*if (subThorttle>330) {
noamnahum 1:f0e37d1d1452 236 if (errorcounter == 0) {
noamnahum 1:f0e37d1d1452 237 t.start();
noamnahum 1:f0e37d1d1452 238 errorcounter = 1;
noamnahum 1:f0e37d1d1452 239 }
noamnahum 1:f0e37d1d1452 240 if (t.read() > 0.1) {
noamnahum 1:f0e37d1d1452 241 t.stop();
noamnahum 1:f0e37d1d1452 242 t.reset();
noamnahum 2:0d44b696736b 243 terrorflag = 1;
noamnahum 4:d40399d8ae44 244 errorcounter = 1;
noamnahum 4:d40399d8ae44 245 sendError();
noamnahum 1:f0e37d1d1452 246 }
noamnahum 6:13f47e31348d 247 }*/
noamnahum 1:f0e37d1d1452 248 }
noamnahum 1:f0e37d1d1452 249 int main() {
noamnahum 2:0d44b696736b 250 pc.baud(9600);
noamnahum 1:f0e37d1d1452 251 pid.start();
noamnahum 2:0d44b696736b 252 main_ticker.attach(readSensors, 0.0005);
noamnahum 1:f0e37d1d1452 253 while(1){
noamnahum 4:d40399d8ae44 254 pc.printf("Pedal: %.4f\n\n\r, Thorttle: %.4f\n\n\r, subthorttle: %.4f\n\n\r, recievecounter: %d\n\n\r, ThorttleErrorFlag: %d\n\n\r, PedalErrorFlag: %d\n\n\r, CanTimeoutFlag: %d\n\n\r", pdagree, mdagree, subThorttle, recievecounter, terrorflag, perrorflag, cantimeoutflag);
noamnahum 1:f0e37d1d1452 255 }
drorbalbul 0:d6eafce8e991 256 }