Noam Nahum / Mbed 2 deprecated CANrecive

FORARKADI

Dependencies:   mbed mypidror1 Motor Map

recive.cpp@2:0d44b696736b, 2020-03-05 (annotated)

Committer:
noamnahum
Date:
Thu Mar 05 16:49:12 2020 +0000
Revision:
2:0d44b696736b
Parent:
1:f0e37d1d1452
Child:
3:94eac3bdfbf5
for noam

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