Noam Nahum / Mbed 2 deprecated CANrecive

FORARKADI

Dependencies:   mbed mypidror1 Motor Map

recive.cpp@8:4b5f04325cbe, 2020-04-30 (annotated)

Committer:
noamnahum
Date:
Thu Apr 30 08:16:51 2020 +0000
Revision:
8:4b5f04325cbe
Parent:
7:34118a0098e8 
CANreceive no Subthorttle check;

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