Noam Nahum / Mbed 2 deprecated CANrecive

FORARKADI

Dependencies:   mbed mypidror1 Motor Map

recive.cpp@1:f0e37d1d1452, 2020-01-07 (annotated)

Committer:
noamnahum
Date:
Tue Jan 07 09:03:33 2020 +0000
Revision:
1:f0e37d1d1452
Parent:
0:d6eafce8e991
Child:
2:0d44b696736b
RECIEVEARKADI

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 1:f0e37d1d1452 41
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 1:f0e37d1d1452 68
noamnahum 1:f0e37d1d1452 69 ///////////////
noamnahum 1:f0e37d1d1452 70 // variables //
noamnahum 1:f0e37d1d1452 71 ///////////////
noamnahum 1:f0e37d1d1452 72 int startup = 0;
noamnahum 1:f0e37d1d1452 73 int counter = 0;
noamnahum 1:f0e37d1d1452 74 int errorcounter = 0;
noamnahum 1:f0e37d1d1452 75
noamnahum 1:f0e37d1d1452 76 // PID
noamnahum 1:f0e37d1d1452 77
noamnahum 1:f0e37d1d1452 78 // pid output limits
noamnahum 1:f0e37d1d1452 79
noamnahum 1:f0e37d1d1452 80 const float output_lower_limit = -255;
noamnahum 1:f0e37d1d1452 81 const float output_upper_limit = 255;
noamnahum 1:f0e37d1d1452 82
noamnahum 1:f0e37d1d1452 83 //pid constants
noamnahum 1:f0e37d1d1452 84
noamnahum 1:f0e37d1d1452 85 const float kp = 1.2;
noamnahum 1:f0e37d1d1452 86 const float ki = 0.5;
noamnahum 1:f0e37d1d1452 87 const float kd = 3;
noamnahum 1:f0e37d1d1452 88
noamnahum 1:f0e37d1d1452 89 //Time sample
noamnahum 1:f0e37d1d1452 90
noamnahum 1:f0e37d1d1452 91 const float Ts = 0.001;
noamnahum 1:f0e37d1d1452 92
noamnahum 1:f0e37d1d1452 93 //Thorttle and pedal intital values
noamnahum 1:f0e37d1d1452 94 float thorttle1, thorttle2, sumThorttle, subThorttle;
noamnahum 1:f0e37d1d1452 95 float mdagree, pdagree = 0;
noamnahum 1:f0e37d1d1452 96 float mythorttle,sumthorttle = 0;
noamnahum 1:f0e37d1d1452 97
noamnahum 1:f0e37d1d1452 98 //Motor speed pwm -1<speed<1
noamnahum 1:f0e37d1d1452 99
noamnahum 1:f0e37d1d1452 100 float speed = 0;
noamnahum 1:f0e37d1d1452 101
noamnahum 1:f0e37d1d1452 102 // Define PID object //
noamnahum 1:f0e37d1d1452 103
noamnahum 1:f0e37d1d1452 104 PID pid(&pdagree, &mdagree, &speed, output_lower_limit, output_upper_limit,kp, ki, kd, Ts);
noamnahum 1:f0e37d1d1452 105
noamnahum 1:f0e37d1d1452 106 //////////////////
noamnahum 1:f0e37d1d1452 107 //define Mapping//
noamnahum 1:f0e37d1d1452 108 //////////////////
noamnahum 1:f0e37d1d1452 109
noamnahum 1:f0e37d1d1452 110 Map mapvaltovolt = Map(0, 1, 0, 3300); // Maping Volt value from 0-1 to 0-3300
noamnahum 1:f0e37d1d1452 111 Map mtodagree = Map(490, 3150, 0, 255); // Maping Volt value from 490-3150 to 0 - 255
noamnahum 1:f0e37d1d1452 112 Map nspeed = Map(-255, 255, -1, 1); // Maping out put speed from -255-255 to -1-1
noamnahum 1:f0e37d1d1452 113
noamnahum 1:f0e37d1d1452 114
noamnahum 1:f0e37d1d1452 115 ////////////////////
noamnahum 1:f0e37d1d1452 116 //define functions///
noamnahum 1:f0e37d1d1452 117 ////////////////////
noamnahum 1:f0e37d1d1452 118
noamnahum 1:f0e37d1d1452 119 // After pid calculate and Sensor data recieve this function command the output to the motor
noamnahum 1:f0e37d1d1452 120
noamnahum 1:f0e37d1d1452 121 void BrakeOn() {
noamnahum 1:f0e37d1d1452 122 mybrake = 1;
noamnahum 1:f0e37d1d1452 123 }
noamnahum 1:f0e37d1d1452 124 void BrakeOff() {
noamnahum 1:f0e37d1d1452 125 mybrake = 0;
noamnahum 1:f0e37d1d1452 126 }
noamnahum 1:f0e37d1d1452 127 void commandMotor(){
noamnahum 1:f0e37d1d1452 128 float Motorcommand = nspeed.Calculate(speed);
noamnahum 1:f0e37d1d1452 129 myMotor.speed(Motorcommand);
noamnahum 1:f0e37d1d1452 130 }
noamnahum 1:f0e37d1d1452 131
noamnahum 1:f0e37d1d1452 132 // if there is Error this function Turn the Motor speed to 0 // Close the Thorttle and then The engine is off
noamnahum 1:f0e37d1d1452 133 void sendError() {
noamnahum 1:f0e37d1d1452 134 myMotor.speed(0);
noamnahum 1:f0e37d1d1452 135 //pid.stop();
noamnahum 1:f0e37d1d1452 136 while (1) {
noamnahum 1:f0e37d1d1452 137 BrakeOn();
noamnahum 1:f0e37d1d1452 138 //pc.printf("Error is: %.4f\n\r", 1);
noamnahum 1:f0e37d1d1452 139 wait(1);
noamnahum 1:f0e37d1d1452 140 BrakeOff();
noamnahum 1:f0e37d1d1452 141 wait(1);
noamnahum 1:f0e37d1d1452 142 }
noamnahum 1:f0e37d1d1452 143 }
noamnahum 1:f0e37d1d1452 144
noamnahum 1:f0e37d1d1452 145 // get Data overcanbus
noamnahum 1:f0e37d1d1452 146 void getData() {
noamnahum 1:f0e37d1d1452 147 //pc.printf("%d", 1);
drorbalbul 0:d6eafce8e991 148 CANMessage msg;
drorbalbul 0:d6eafce8e991 149 if(can1.read(msg)) {
noamnahum 1:f0e37d1d1452 150 if (can.read() > 0) {
noamnahum 1:f0e37d1d1452 151 can.stop();
noamnahum 1:f0e37d1d1452 152 can.reset();
noamnahum 1:f0e37d1d1452 153 }
noamnahum 1:f0e37d1d1452 154 //pc.printf("%d", msg.data[0]);
noamnahum 1:f0e37d1d1452 155 if (msg.data[2]==1){
noamnahum 1:f0e37d1d1452 156 sendError();
noamnahum 1:f0e37d1d1452 157 }
noamnahum 1:f0e37d1d1452 158 if (msg.data[3]==1){
noamnahum 1:f0e37d1d1452 159 BrakeOn();
noamnahum 1:f0e37d1d1452 160 }
noamnahum 1:f0e37d1d1452 161 if (msg.data[3]==0){
noamnahum 1:f0e37d1d1452 162 BrakeOff();
noamnahum 1:f0e37d1d1452 163 }
noamnahum 1:f0e37d1d1452 164 if(msg.data[0]<30){
noamnahum 1:f0e37d1d1452 165 pdagree = 30;
noamnahum 1:f0e37d1d1452 166 }
noamnahum 1:f0e37d1d1452 167 if(msg.data[0]>30){
noamnahum 1:f0e37d1d1452 168 pdagree = msg.data[0]*1.5;
noamnahum 1:f0e37d1d1452 169 }
noamnahum 1:f0e37d1d1452 170 //pc.printf("Subthorttle is: %.4f\n\r", pdagree);
noamnahum 1:f0e37d1d1452 171 //pc.printf("Error is: %.4f\n\r", msg.data[2]);
noamnahum 1:f0e37d1d1452 172 }
noamnahum 1:f0e37d1d1452 173 if (!can1.read(msg)) {
noamnahum 1:f0e37d1d1452 174 can.start();
noamnahum 1:f0e37d1d1452 175 }
noamnahum 1:f0e37d1d1452 176 if (can.read() > 0.1) { // if message not recieved more then 0.1 sec send error
noamnahum 1:f0e37d1d1452 177 can.stop();
noamnahum 1:f0e37d1d1452 178 can.reset();
noamnahum 1:f0e37d1d1452 179 sendError();
noamnahum 1:f0e37d1d1452 180 }
noamnahum 1:f0e37d1d1452 181 }
noamnahum 1:f0e37d1d1452 182
noamnahum 1:f0e37d1d1452 183 //reading Thorttle sensors
noamnahum 1:f0e37d1d1452 184
noamnahum 1:f0e37d1d1452 185 void readSensors() {
noamnahum 1:f0e37d1d1452 186 thorttle1 = analog_value1.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
noamnahum 1:f0e37d1d1452 187 thorttle2 = analog_value2.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
noamnahum 1:f0e37d1d1452 188 thorttle1 = mapvaltovolt.Calculate(thorttle1);
noamnahum 1:f0e37d1d1452 189 thorttle2 = mapvaltovolt.Calculate(thorttle2);
noamnahum 1:f0e37d1d1452 190 sumThorttle = thorttle1+thorttle2;
noamnahum 1:f0e37d1d1452 191 subThorttle = abs(3500-sumThorttle);
noamnahum 1:f0e37d1d1452 192 //pc.printf("matzeret1 is: %.4f, matzeret2 is:%.4f\n\r", thorttle1, thorttle2);
noamnahum 1:f0e37d1d1452 193 //pc.printf("Subthorttle is: %.4f\n\r", subThorttle);
noamnahum 1:f0e37d1d1452 194 if (subThorttle<350) { //checking error
noamnahum 1:f0e37d1d1452 195 mythorttle = mtodagree.Calculate(thorttle1);
noamnahum 1:f0e37d1d1452 196 sumthorttle = sumthorttle + mythorttle;
noamnahum 1:f0e37d1d1452 197 counter++;
noamnahum 1:f0e37d1d1452 198 if (t.read()>0) {
noamnahum 1:f0e37d1d1452 199 errorcounter = 0;
noamnahum 1:f0e37d1d1452 200 t.stop();
noamnahum 1:f0e37d1d1452 201 t.reset();
noamnahum 1:f0e37d1d1452 202 }
noamnahum 1:f0e37d1d1452 203 if (counter == 10) {
noamnahum 1:f0e37d1d1452 204 mdagree = sumthorttle/10;
noamnahum 1:f0e37d1d1452 205 getData();
noamnahum 1:f0e37d1d1452 206 counter = 0;
noamnahum 1:f0e37d1d1452 207 sumthorttle = 0;
noamnahum 1:f0e37d1d1452 208 }
noamnahum 1:f0e37d1d1452 209 }
noamnahum 1:f0e37d1d1452 210 if (subThorttle>350) {
noamnahum 1:f0e37d1d1452 211 if (errorcounter == 0) {
noamnahum 1:f0e37d1d1452 212 t.start();
noamnahum 1:f0e37d1d1452 213 errorcounter = 1;
noamnahum 1:f0e37d1d1452 214 }
noamnahum 1:f0e37d1d1452 215 if (t.read() > 0.1) {
noamnahum 1:f0e37d1d1452 216 t.stop();
noamnahum 1:f0e37d1d1452 217 t.reset();
noamnahum 1:f0e37d1d1452 218 sendError();
noamnahum 1:f0e37d1d1452 219 }
noamnahum 1:f0e37d1d1452 220 }
noamnahum 1:f0e37d1d1452 221 }
noamnahum 1:f0e37d1d1452 222 int main() {
noamnahum 1:f0e37d1d1452 223 pid.start();
noamnahum 1:f0e37d1d1452 224 while(1){
noamnahum 1:f0e37d1d1452 225 readSensors();
noamnahum 1:f0e37d1d1452 226 pid.sample();
noamnahum 1:f0e37d1d1452 227 commandMotor();
noamnahum 1:f0e37d1d1452 228 //wait(Ts);
noamnahum 1:f0e37d1d1452 229 }
drorbalbul 0:d6eafce8e991 230 }