Noam Nahum / Mbed 2 deprecated CANrecive

FORARKADI

Dependencies:   mbed mypidror1 Motor Map

recive.cpp@7:34118a0098e8, 2020-03-13 (annotated)

Committer:
noamnahum
Date:
Fri Mar 13 12:35:04 2020 +0000
Revision:
7:34118a0098e8
Parent:
6:13f47e31348d
Child:
8:4b5f04325cbe
FOR DROR1;

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 7:34118a0098e8 54 //Xbee
noamnahum 7:34118a0098e8 55
noamnahum 7:34118a0098e8 56 BufferedSerial xbee(PA_9, PA_10);
noamnahum 7:34118a0098e8 57
noamnahum 1:f0e37d1d1452 58 //Motor Control
noamnahum 1:f0e37d1d1452 59
noamnahum 1:f0e37d1d1452 60 Motor myMotor(PA_8, PA_5, PA_6);
noamnahum 1:f0e37d1d1452 61
noamnahum 1:f0e37d1d1452 62 //Brake signal
noamnahum 1:f0e37d1d1452 63
noamnahum 1:f0e37d1d1452 64 //Serial
noamnahum 1:f0e37d1d1452 65
noamnahum 1:f0e37d1d1452 66 Serial pc(USBTX,USBRX);
noamnahum 1:f0e37d1d1452 67
noamnahum 1:f0e37d1d1452 68 //Timer
noamnahum 1:f0e37d1d1452 69
noamnahum 1:f0e37d1d1452 70 Timer t;
noamnahum 1:f0e37d1d1452 71 Timer can;
noamnahum 2:0d44b696736b 72 Timer tpid;
noamnahum 1:f0e37d1d1452 73
noamnahum 1:f0e37d1d1452 74 ///////////////
noamnahum 1:f0e37d1d1452 75 // variables //
noamnahum 1:f0e37d1d1452 76 ///////////////
noamnahum 1:f0e37d1d1452 77 int startup = 0;
noamnahum 1:f0e37d1d1452 78 int counter = 0;
noamnahum 1:f0e37d1d1452 79 int errorcounter = 0;
noamnahum 3:94eac3bdfbf5 80 int recievecounter = 0;
noamnahum 1:f0e37d1d1452 81
noamnahum 1:f0e37d1d1452 82 // PID
noamnahum 1:f0e37d1d1452 83
noamnahum 1:f0e37d1d1452 84 // pid output limits
noamnahum 1:f0e37d1d1452 85
noamnahum 1:f0e37d1d1452 86 const float output_lower_limit = -255;
noamnahum 1:f0e37d1d1452 87 const float output_upper_limit = 255;
noamnahum 1:f0e37d1d1452 88
noamnahum 1:f0e37d1d1452 89 //pid constants
noamnahum 1:f0e37d1d1452 90
noamnahum 2:0d44b696736b 91 const float kp = 1.5;
noamnahum 5:8ef69c6f3718 92 const float ki = 3;
noamnahum 2:0d44b696736b 93 const float kd = 0;
noamnahum 1:f0e37d1d1452 94
noamnahum 1:f0e37d1d1452 95 //Time sample
noamnahum 1:f0e37d1d1452 96
noamnahum 1:f0e37d1d1452 97 const float Ts = 0.001;
noamnahum 1:f0e37d1d1452 98
noamnahum 2:0d44b696736b 99 //Tickers
noamnahum 2:0d44b696736b 100 Ticker main_ticker;
noamnahum 2:0d44b696736b 101 Ticker motor;
noamnahum 3:94eac3bdfbf5 102 Ticker syserror;
noamnahum 2:0d44b696736b 103
noamnahum 2:0d44b696736b 104 //Flags for indication if something happend - error handling
noamnahum 2:0d44b696736b 105 int pidflag = 0;
noamnahum 2:0d44b696736b 106 int canflag = 0;
noamnahum 2:0d44b696736b 107 int cantimeoutflag = 0; // if can message is not recived more then 100ms
noamnahum 2:0d44b696736b 108 int rsflag = 0;
noamnahum 2:0d44b696736b 109 int terrorflag = 0;
noamnahum 2:0d44b696736b 110 int perrorflag = 0;
noamnahum 2:0d44b696736b 111
noamnahum 1:f0e37d1d1452 112 //Thorttle and pedal intital values
noamnahum 1:f0e37d1d1452 113 float thorttle1, thorttle2, sumThorttle, subThorttle;
noamnahum 1:f0e37d1d1452 114 float mdagree, pdagree = 0;
noamnahum 1:f0e37d1d1452 115 float mythorttle,sumthorttle = 0;
noamnahum 1:f0e37d1d1452 116
noamnahum 1:f0e37d1d1452 117 //Motor speed pwm -1<speed<1
noamnahum 1:f0e37d1d1452 118
noamnahum 1:f0e37d1d1452 119 float speed = 0;
noamnahum 1:f0e37d1d1452 120
noamnahum 1:f0e37d1d1452 121 // Define PID object //
noamnahum 1:f0e37d1d1452 122
noamnahum 1:f0e37d1d1452 123 PID pid(&pdagree, &mdagree, &speed, output_lower_limit, output_upper_limit,kp, ki, kd, Ts);
noamnahum 1:f0e37d1d1452 124
noamnahum 1:f0e37d1d1452 125 //////////////////
noamnahum 1:f0e37d1d1452 126 //define Mapping//
noamnahum 1:f0e37d1d1452 127 //////////////////
noamnahum 1:f0e37d1d1452 128
noamnahum 1:f0e37d1d1452 129 Map mapvaltovolt = Map(0, 1, 0, 3300); // Maping Volt value from 0-1 to 0-3300
noamnahum 2:0d44b696736b 130 Map mtodagree = Map(230, 3070, 0, 255); // Maping Volt value from 490-3150 to 0 - 255
noamnahum 1:f0e37d1d1452 131 Map nspeed = Map(-255, 255, -1, 1); // Maping out put speed from -255-255 to -1-1
noamnahum 1:f0e37d1d1452 132
noamnahum 1:f0e37d1d1452 133
noamnahum 1:f0e37d1d1452 134 ////////////////////
noamnahum 1:f0e37d1d1452 135 //define functions///
noamnahum 1:f0e37d1d1452 136 ////////////////////
noamnahum 1:f0e37d1d1452 137
noamnahum 1:f0e37d1d1452 138 // After pid calculate and Sensor data recieve this function command the output to the motor
noamnahum 1:f0e37d1d1452 139
noamnahum 1:f0e37d1d1452 140 void BrakeOn() {
noamnahum 1:f0e37d1d1452 141 mybrake = 1;
noamnahum 1:f0e37d1d1452 142 }
noamnahum 1:f0e37d1d1452 143 void BrakeOff() {
noamnahum 1:f0e37d1d1452 144 mybrake = 0;
noamnahum 1:f0e37d1d1452 145 }
noamnahum 1:f0e37d1d1452 146 void commandMotor(){
noamnahum 1:f0e37d1d1452 147 float Motorcommand = nspeed.Calculate(speed);
noamnahum 1:f0e37d1d1452 148 myMotor.speed(Motorcommand);
noamnahum 1:f0e37d1d1452 149 }
noamnahum 1:f0e37d1d1452 150
noamnahum 1:f0e37d1d1452 151 // if there is Error this function Turn the Motor speed to 0 // Close the Thorttle and then The engine is off
noamnahum 1:f0e37d1d1452 152 void sendError() {
noamnahum 3:94eac3bdfbf5 153 if (errorcounter == 1){
noamnahum 3:94eac3bdfbf5 154 main_ticker.detach();
noamnahum 3:94eac3bdfbf5 155 pid.stop();
noamnahum 3:94eac3bdfbf5 156 syserror.attach(sendError, 0.0005);
noamnahum 3:94eac3bdfbf5 157 }
noamnahum 1:f0e37d1d1452 158 myMotor.speed(0);
noamnahum 2:0d44b696736b 159 }
noamnahum 2:0d44b696736b 160 /*while (1) {
noamnahum 1:f0e37d1d1452 161 BrakeOn();
noamnahum 1:f0e37d1d1452 162 //pc.printf("Error is: %.4f\n\r", 1);
noamnahum 1:f0e37d1d1452 163 wait(1);
noamnahum 1:f0e37d1d1452 164 BrakeOff();
noamnahum 1:f0e37d1d1452 165 wait(1);
noamnahum 1:f0e37d1d1452 166 }
noamnahum 2:0d44b696736b 167 }*/
noamnahum 1:f0e37d1d1452 168
noamnahum 1:f0e37d1d1452 169 // get Data overcanbus
noamnahum 1:f0e37d1d1452 170 void getData() {
noamnahum 1:f0e37d1d1452 171 //pc.printf("%d", 1);
drorbalbul 0:d6eafce8e991 172 CANMessage msg;
drorbalbul 0:d6eafce8e991 173 if(can1.read(msg)) {
noamnahum 3:94eac3bdfbf5 174 recievecounter ++;
noamnahum 2:0d44b696736b 175 //pc.printf("Pedal sensor worng values\n");
noamnahum 3:94eac3bdfbf5 176 //pc.printf("my data: %d", msg.data[2]);
noamnahum 1:f0e37d1d1452 177 if (can.read() > 0) {
noamnahum 1:f0e37d1d1452 178 can.stop();
noamnahum 1:f0e37d1d1452 179 can.reset();
noamnahum 1:f0e37d1d1452 180 }
noamnahum 1:f0e37d1d1452 181 if (msg.data[2]==1){
noamnahum 3:94eac3bdfbf5 182 errorcounter = 1;
noamnahum 2:0d44b696736b 183 perrorflag = 1;
noamnahum 4:d40399d8ae44 184 sendError();
noamnahum 1:f0e37d1d1452 185 }
noamnahum 3:94eac3bdfbf5 186 if (msg.data[2]==0){
noamnahum 3:94eac3bdfbf5 187 perrorflag = 0;
noamnahum 3:94eac3bdfbf5 188 }
noamnahum 1:f0e37d1d1452 189 if (msg.data[3]==1){
noamnahum 1:f0e37d1d1452 190 BrakeOn();
noamnahum 1:f0e37d1d1452 191 }
noamnahum 1:f0e37d1d1452 192 if (msg.data[3]==0){
noamnahum 1:f0e37d1d1452 193 BrakeOff();
noamnahum 1:f0e37d1d1452 194 }
noamnahum 3:94eac3bdfbf5 195 pdagree = msg.data[0];
noamnahum 1:f0e37d1d1452 196 }
noamnahum 1:f0e37d1d1452 197 if (!can1.read(msg)) {
noamnahum 1:f0e37d1d1452 198 can.start();
noamnahum 1:f0e37d1d1452 199 }
noamnahum 1:f0e37d1d1452 200 if (can.read() > 0.1) { // if message not recieved more then 0.1 sec send error
noamnahum 1:f0e37d1d1452 201 can.stop();
noamnahum 3:94eac3bdfbf5 202 //can.reset();
noamnahum 4:d40399d8ae44 203 errorcounter = 1;
noamnahum 2:0d44b696736b 204 cantimeoutflag = 1;
noamnahum 4:d40399d8ae44 205 sendError();
noamnahum 1:f0e37d1d1452 206 }
noamnahum 1:f0e37d1d1452 207 }
noamnahum 2:0d44b696736b 208
noamnahum 1:f0e37d1d1452 209
noamnahum 1:f0e37d1d1452 210 //reading Thorttle sensors
noamnahum 1:f0e37d1d1452 211
noamnahum 1:f0e37d1d1452 212 void readSensors() {
noamnahum 1:f0e37d1d1452 213 thorttle1 = analog_value1.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
noamnahum 1:f0e37d1d1452 214 thorttle2 = analog_value2.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
noamnahum 1:f0e37d1d1452 215 thorttle1 = mapvaltovolt.Calculate(thorttle1);
noamnahum 1:f0e37d1d1452 216 thorttle2 = mapvaltovolt.Calculate(thorttle2);
noamnahum 1:f0e37d1d1452 217 sumThorttle = thorttle1+thorttle2;
noamnahum 3:94eac3bdfbf5 218 subThorttle = abs(3300-sumThorttle);
noamnahum 2:0d44b696736b 219 getData();
noamnahum 2:0d44b696736b 220 pid.sample();
noamnahum 2:0d44b696736b 221 commandMotor();
noamnahum 1:f0e37d1d1452 222 //pc.printf("matzeret1 is: %.4f, matzeret2 is:%.4f\n\r", thorttle1, thorttle2);
noamnahum 1:f0e37d1d1452 223 //pc.printf("Subthorttle is: %.4f\n\r", subThorttle);
noamnahum 6:13f47e31348d 224 //checking error
noamnahum 6:13f47e31348d 225 mythorttle = mtodagree.Calculate(thorttle1);
noamnahum 6:13f47e31348d 226 sumthorttle = sumthorttle + mythorttle;
noamnahum 6:13f47e31348d 227 counter++;
noamnahum 6:13f47e31348d 228 if (t.read()>0) {
noamnahum 6:13f47e31348d 229 errorcounter = 0;
noamnahum 6:13f47e31348d 230 t.stop();
noamnahum 6:13f47e31348d 231 t.reset();
noamnahum 1:f0e37d1d1452 232 }
noamnahum 6:13f47e31348d 233 if (counter == 10) {
noamnahum 6:13f47e31348d 234 mdagree = sumthorttle/10;
noamnahum 6:13f47e31348d 235 counter = 0;
noamnahum 6:13f47e31348d 236 sumthorttle = 0;
noamnahum 6:13f47e31348d 237 }
noamnahum 6:13f47e31348d 238
noamnahum 6:13f47e31348d 239 /*if (subThorttle>330) {
noamnahum 1:f0e37d1d1452 240 if (errorcounter == 0) {
noamnahum 1:f0e37d1d1452 241 t.start();
noamnahum 1:f0e37d1d1452 242 errorcounter = 1;
noamnahum 1:f0e37d1d1452 243 }
noamnahum 1:f0e37d1d1452 244 if (t.read() > 0.1) {
noamnahum 1:f0e37d1d1452 245 t.stop();
noamnahum 1:f0e37d1d1452 246 t.reset();
noamnahum 2:0d44b696736b 247 terrorflag = 1;
noamnahum 4:d40399d8ae44 248 errorcounter = 1;
noamnahum 4:d40399d8ae44 249 sendError();
noamnahum 1:f0e37d1d1452 250 }
noamnahum 6:13f47e31348d 251 }*/
noamnahum 1:f0e37d1d1452 252 }
noamnahum 1:f0e37d1d1452 253 int main() {
noamnahum 7:34118a0098e8 254 pc.baud(57600);
noamnahum 7:34118a0098e8 255 xbee.baud(57600);
noamnahum 1:f0e37d1d1452 256 pid.start();
noamnahum 2:0d44b696736b 257 main_ticker.attach(readSensors, 0.0005);
noamnahum 1:f0e37d1d1452 258 while(1){
noamnahum 7:34118a0098e8 259 //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 7:34118a0098e8 260 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 261 }
drorbalbul 0:d6eafce8e991 262 }