University of Texas Solar Vehicles Team / Mbed 2 deprecated motor-control

Dependencies:   mbed

Committer:
armin1376
Date:
Thu Nov 14 00:31:13 2019 +0000
Revision:
7:3352a96997b3
Parent:
6:e7c29e46ad6c
Shows speed and current values but not temp

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jjohnle 3:f282664610ba 1 // Motor Control Board Program
jjohnle 3:f282664610ba 2 // This program operates the Tritium controller and also sends data
jjohnle 3:f282664610ba 3 // over the separate car CAN.
jjohnle 4:7d027ceba0a0 4
jjohnle 2:2a4822c7c91a 5 #include "mbed.h"
jjohnle 4:7d027ceba0a0 6
jjohnle 3:f282664610ba 7 // CAN base address and offsets (Tritium)
jjohnle 0:c61d18b01f8c 8 #define DC_BASE 0x220 // Driver controls base address
jjohnle 0:c61d18b01f8c 9 #define DC_DRIVE 0x01 // Offset for motor drive command
jjohnle 0:c61d18b01f8c 10 #define DC_POWER 0x02 // Offset for motor power command
jjohnle 0:c61d18b01f8c 11 #define DC_RESET 0x03 // Offset for reset command
jjohnle 0:c61d18b01f8c 12 #define DC_SWITCH 0x04 // Offset for phase current measurement
jjohnle 4:7d027ceba0a0 13
jjohnle 3:f282664610ba 14 #define MC_BASE 0x240 // Motor controls base address
jjohnle 3:f282664610ba 15 #define MC_BUS 0x02 // Bus measurement offset
jjohnle 3:f282664610ba 16 #define MC_VEL 0x03 // Velocity measurement offset
jjohnle 3:f282664610ba 17 #define MC_PHCUR 0x04 // Phase Current offset
jjohnle 3:f282664610ba 18 #define MC_VOVEC 0x05 // Voltage Vector offset
jjohnle 3:f282664610ba 19 #define MC_CUVEC 0x06 // current vector offset
jjohnle 3:f282664610ba 20 #define MC_BEMF 0x07 // back emf offset
jjohnle 3:f282664610ba 21 #define MC_TEMP 0x0B // heat sink and motor temp offset
jjohnle 3:f282664610ba 22 #define MC_AMPH 0x0E // odometer and bus amp ohours measuremeant
jjohnle 0:c61d18b01f8c 23 #define MAX_VELOCITY 100 // motor velocity in m/s
jjohnle 0:c61d18b01f8c 24 #define MAX_CURRENT 1.0 // desired motor current as percentage of max current
jjohnle 4:7d027ceba0a0 25
jjohnle 3:f282664610ba 26 #define DC_BUS_CURRENT 0x900
jjohnle 3:f282664610ba 27 #define DC_BUS_VOLTAGE 0x901
jjohnle 3:f282664610ba 28 #define PHASE_B_CURRENT 0x902
jjohnle 3:f282664610ba 29 #define PHASE_C_CURRENT 0x903
jjohnle 3:f282664610ba 30 #define VEHICLE_VELOCITY 0x904
jjohnle 3:f282664610ba 31 #define MOTOR_VELOCITY 0x905
jjohnle 3:f282664610ba 32 #define VD 0x906
jjohnle 3:f282664610ba 33 #define VQ 0x907
jjohnle 3:f282664610ba 34 #define ID 0x908
jjohnle 3:f282664610ba 35 #define IQ 0x909
jjohnle 3:f282664610ba 36 #define BEMFD 0x90A
jjohnle 3:f282664610ba 37 #define BEMFQ 0x90B
jjohnle 3:f282664610ba 38 #define HEAT_SINK_TEMPERATURE 0x90C
jjohnle 3:f282664610ba 39 #define MOTOR_TEMPERATURE 0x90D
jjohnle 3:f282664610ba 40 #define DC_BUS_AMP_HOURS 0x90E
jjohnle 3:f282664610ba 41 #define ODOMETER 0x90F
armin1376 5:f55622eacb87 42
armin1376 5:f55622eacb87 43 #define DASH_Forward 0x580 // DASH Forward Motor ID
armin1376 5:f55622eacb87 44 #define DASH_Cruise_Set 0x581 // DASH Brake ID
armin1376 5:f55622eacb87 45 #define DASH_Cruise 0x582 // DASH Brake ID
armin1376 5:f55622eacb87 46 #define DASH_Regen 0x583 // DASH Regen ID
jjohnle 4:7d027ceba0a0 47
jjohnle 3:f282664610ba 48 CAN can1(PD_0,PD_1,125000); // can1 is car CAN (Rx, Tx, speed)
armin1376 5:f55622eacb87 49 CAN can2(PB_5,PB_6, 100000); // can2 is motor controller CAN (Rx, Tx, speed)
jjohnle 0:c61d18b01f8c 50 Serial pc(USBTX, USBRX);
jjohnle 4:7d027ceba0a0 51
jjohnle 3:f282664610ba 52 AnalogIn ain(PB_0);
jjohnle 4:7d027ceba0a0 53
jjohnle 3:f282664610ba 54 DigitalOut LED8(PF_2);
jjohnle 3:f282664610ba 55 DigitalOut LED7(PA_7);
jjohnle 3:f282664610ba 56 DigitalOut LED6(PF_10);
jjohnle 3:f282664610ba 57 DigitalOut LED5(PF_5);
jjohnle 3:f282664610ba 58 DigitalOut LED4a(PF_3);
jjohnle 3:f282664610ba 59 DigitalOut LED3a(PC_3);
jjohnle 3:f282664610ba 60 DigitalOut LED2a(PC_0);
jjohnle 3:f282664610ba 61 DigitalOut LED1a(PA_3);
armin1376 6:e7c29e46ad6c 62
armin1376 6:e7c29e46ad6c 63
armin1376 6:e7c29e46ad6c 64 CANMessage msg;
armin1376 6:e7c29e46ad6c 65
armin1376 6:e7c29e46ad6c 66 union {
armin1376 6:e7c29e46ad6c 67 char rcvdata[4];
armin1376 6:e7c29e46ad6c 68 float rxdata;
armin1376 6:e7c29e46ad6c 69 } urxdata;
armin1376 6:e7c29e46ad6c 70
armin1376 6:e7c29e46ad6c 71 char rdata2[8];
armin1376 6:e7c29e46ad6c 72 float vehicleVel;
armin1376 6:e7c29e46ad6c 73 float motorVel;
jjohnle 4:7d027ceba0a0 74
jjohnle 3:f282664610ba 75 #define MAX_VELOCITY 100 // motor velocity in m/s
jjohnle 3:f282664610ba 76 #define MAX_CURRENT 1.0 // desired motor current as percentage of max current
armin1376 6:e7c29e46ad6c 77 void speeed(){
armin1376 6:e7c29e46ad6c 78
armin1376 6:e7c29e46ad6c 79
armin1376 5:f55622eacb87 80 while(1){
armin1376 7:3352a96997b3 81
armin1376 6:e7c29e46ad6c 82 if(can2.read(msg) && msg.id == (MC_BASE+MC_VEL)) {
armin1376 6:e7c29e46ad6c 83 for (int i = 0; i < msg.len; i++) {
armin1376 6:e7c29e46ad6c 84 rdata2[i] = msg.data[i];
armin1376 6:e7c29e46ad6c 85 }
armin1376 6:e7c29e46ad6c 86 }
armin1376 6:e7c29e46ad6c 87
armin1376 6:e7c29e46ad6c 88 if(msg.id == (MC_BASE+MC_VEL)) {
armin1376 6:e7c29e46ad6c 89 for (int i = 0; i < msg.len; i++) {
armin1376 6:e7c29e46ad6c 90 rdata2[i] = msg.data[i];
armin1376 6:e7c29e46ad6c 91 }
armin1376 6:e7c29e46ad6c 92 // sending value to CAR can
armin1376 6:e7c29e46ad6c 93 if (!can1.write(CANMessage(0x243, (char*) rdata2, 8))) {
armin1376 6:e7c29e46ad6c 94 pc.printf("Cannot write to CAN\n");
armin1376 6:e7c29e46ad6c 95 }
armin1376 6:e7c29e46ad6c 96 wait(0.001);
armin1376 6:e7c29e46ad6c 97
armin1376 6:e7c29e46ad6c 98 urxdata.rcvdata[3] = rdata2[7];
armin1376 6:e7c29e46ad6c 99 urxdata.rcvdata[2] = rdata2[6];
armin1376 6:e7c29e46ad6c 100 urxdata.rcvdata[1] = rdata2[5];
armin1376 6:e7c29e46ad6c 101 urxdata.rcvdata[0] = rdata2[4];
armin1376 6:e7c29e46ad6c 102 vehicleVel = urxdata.rxdata;
armin1376 6:e7c29e46ad6c 103 urxdata.rcvdata[3] = rdata2[3];
armin1376 6:e7c29e46ad6c 104 urxdata.rcvdata[2] = rdata2[2];
armin1376 6:e7c29e46ad6c 105 urxdata.rcvdata[1] = rdata2[1];
armin1376 6:e7c29e46ad6c 106 urxdata.rcvdata[0] = rdata2[0];
armin1376 6:e7c29e46ad6c 107 motorVel = urxdata.rxdata;
armin1376 6:e7c29e46ad6c 108 }
armin1376 5:f55622eacb87 109 }
armin1376 5:f55622eacb87 110
armin1376 5:f55622eacb87 111 }
armin1376 5:f55622eacb87 112
armin1376 6:e7c29e46ad6c 113
armin1376 6:e7c29e46ad6c 114
armin1376 5:f55622eacb87 115 int main() {
armin1376 6:e7c29e46ad6c 116
armin1376 6:e7c29e46ad6c 117 can1.frequency(125000);
armin1376 6:e7c29e46ad6c 118 Thread V1(speeed);
armin1376 7:3352a96997b3 119
armin1376 5:f55622eacb87 120
jjohnle 3:f282664610ba 121 float current = MAX_CURRENT;
jjohnle 3:f282664610ba 122 float velocity = MAX_VELOCITY;
jjohnle 3:f282664610ba 123 float bus_current = MAX_CURRENT;
jjohnle 3:f282664610ba 124 float DCbuscur;
jjohnle 3:f282664610ba 125 float DCbusvolt;
jjohnle 3:f282664610ba 126 double pedal_position;
jjohnle 3:f282664610ba 127 float data[2];
jjohnle 3:f282664610ba 128 float data2[2];
jjohnle 3:f282664610ba 129 float meas;
jjohnle 3:f282664610ba 130 double avgval;
jjohnle 3:f282664610ba 131 int n;
jjohnle 3:f282664610ba 132 int dummy;
jjohnle 3:f282664610ba 133 int alive;
jjohnle 4:7d027ceba0a0 134
jjohnle 3:f282664610ba 135 // other ids we need to read
jjohnle 3:f282664610ba 136 float phaseBcurrent;
jjohnle 3:f282664610ba 137 float phaseCcurrent;
armin1376 6:e7c29e46ad6c 138
armin1376 6:e7c29e46ad6c 139
jjohnle 3:f282664610ba 140 float vd;
jjohnle 3:f282664610ba 141 float vq;
jjohnle 3:f282664610ba 142 float Id;
jjohnle 3:f282664610ba 143 float Iq;
jjohnle 3:f282664610ba 144 float BEMFd;
jjohnle 3:f282664610ba 145 float BEMFq;
jjohnle 3:f282664610ba 146 float heatSinkTemp;
jjohnle 3:f282664610ba 147 float motorTemp;
jjohnle 3:f282664610ba 148 float DCBusAmpHours;
jjohnle 3:f282664610ba 149 float odometerValue;
armin1376 5:f55622eacb87 150 int forward;
armin1376 5:f55622eacb87 151 int cruiseSet;
armin1376 5:f55622eacb87 152 int cruise;
armin1376 5:f55622eacb87 153 float regen;
jjohnle 4:7d027ceba0a0 154
jjohnle 3:f282664610ba 155 //char const * serial = "0002173";
jjohnle 3:f282664610ba 156 // can1.frequency(500000);
jjohnle 3:f282664610ba 157 int id;
jjohnle 3:f282664610ba 158 int id2;
jjohnle 3:f282664610ba 159 int id3;
jjohnle 3:f282664610ba 160 char rdata[8];
armin1376 6:e7c29e46ad6c 161
jjohnle 3:f282664610ba 162 char rdata3[8];
jjohnle 3:f282664610ba 163 char rdata4[8];
jjohnle 3:f282664610ba 164 char rdata5[8];
jjohnle 3:f282664610ba 165 char rdata6[8];
jjohnle 3:f282664610ba 166 char rdata7[8];
jjohnle 3:f282664610ba 167 char rdata8[8];
jjohnle 3:f282664610ba 168 char rdata9[8];
armin1376 5:f55622eacb87 169
armin1376 5:f55622eacb87 170 char ddata[8];
armin1376 5:f55622eacb87 171 char ddata1[8];
armin1376 5:f55622eacb87 172 char ddata2[8];
armin1376 5:f55622eacb87 173 char ddata3[8];
armin1376 5:f55622eacb87 174
armin1376 5:f55622eacb87 175 float zero = 0;
jjohnle 4:7d027ceba0a0 176
jjohnle 3:f282664610ba 177 CANMessage msg;
armin1376 5:f55622eacb87 178 CANMessage msg2;
armin1376 5:f55622eacb87 179
jjohnle 3:f282664610ba 180 dummy = 0;
jjohnle 3:f282664610ba 181 alive = 0;
jjohnle 4:7d027ceba0a0 182
armin1376 6:e7c29e46ad6c 183
jjohnle 4:7d027ceba0a0 184
jjohnle 3:f282664610ba 185 id = DC_BASE + DC_DRIVE;
jjohnle 3:f282664610ba 186 id2 = DC_BASE + DC_POWER;
jjohnle 3:f282664610ba 187 id3 = MC_BASE + DC_POWER;
jjohnle 4:7d027ceba0a0 188
jjohnle 3:f282664610ba 189 while (1) {
jjohnle 3:f282664610ba 190 n = 0;
jjohnle 3:f282664610ba 191 avgval = 0.0;
jjohnle 3:f282664610ba 192 while(n < 100) {
jjohnle 3:f282664610ba 193 meas = ain.read();
jjohnle 3:f282664610ba 194 avgval = avgval + meas;
jjohnle 3:f282664610ba 195 n++ ;
jjohnle 2:2a4822c7c91a 196 }
jjohnle 3:f282664610ba 197 pedal_position = avgval/100.0;
jjohnle 4:7d027ceba0a0 198
jjohnle 3:f282664610ba 199 if(pedal_position > 0.005)
jjohnle 3:f282664610ba 200 LED1a = 1;
jjohnle 3:f282664610ba 201 else
jjohnle 3:f282664610ba 202 LED1a = 0;
jjohnle 3:f282664610ba 203 if(pedal_position > 0.01)
jjohnle 3:f282664610ba 204 LED2a = 1;
jjohnle 3:f282664610ba 205 else
jjohnle 3:f282664610ba 206 LED2a = 0;
jjohnle 3:f282664610ba 207 if(pedal_position > 0.015)
jjohnle 3:f282664610ba 208 LED3a = 1;
jjohnle 3:f282664610ba 209 else
jjohnle 3:f282664610ba 210 LED3a = 0;
jjohnle 3:f282664610ba 211 if(pedal_position > 0.02)
jjohnle 3:f282664610ba 212 LED4a = 1;
jjohnle 3:f282664610ba 213 else
jjohnle 3:f282664610ba 214 LED4a = 0;
jjohnle 3:f282664610ba 215 if(pedal_position > 0.025)
jjohnle 3:f282664610ba 216 LED5 = 1;
jjohnle 3:f282664610ba 217 else
jjohnle 3:f282664610ba 218 LED5 = 0;
jjohnle 3:f282664610ba 219 if(pedal_position > 0.03)
jjohnle 3:f282664610ba 220 LED6 = 1;
jjohnle 3:f282664610ba 221 else
jjohnle 3:f282664610ba 222 LED6 = 0;
jjohnle 3:f282664610ba 223 if(pedal_position > 0.035)
jjohnle 3:f282664610ba 224 LED7 = 1;
jjohnle 3:f282664610ba 225 else
jjohnle 3:f282664610ba 226 LED7 = 0;
jjohnle 4:7d027ceba0a0 227
jjohnle 3:f282664610ba 228 current = MAX_CURRENT * pedal_position;
jjohnle 3:f282664610ba 229 velocity = 9.0;
jjohnle 4:7d027ceba0a0 230
jjohnle 3:f282664610ba 231 data[1] = current; // Flipped because of endianness
jjohnle 3:f282664610ba 232 data[0] = velocity;
jjohnle 4:7d027ceba0a0 233
jjohnle 3:f282664610ba 234 if (!can2.write(CANMessage(id, (char*)data, 8))) // send current and velocity to Tritum
jjohnle 3:f282664610ba 235 printf("Drive failed \n\r");
jjohnle 4:7d027ceba0a0 236
jjohnle 3:f282664610ba 237 data2[1] = bus_current;
jjohnle 3:f282664610ba 238 data2[0] = 0.0;
jjohnle 3:f282664610ba 239 if (!can2.write(CANMessage(id2, (char*)data2, 8)))
jjohnle 3:f282664610ba 240 dummy = 0;
jjohnle 4:7d027ceba0a0 241
jjohnle 3:f282664610ba 242 wait_ms(10); // Need message every 250ms to maintain operation
jjohnle 4:7d027ceba0a0 243
jjohnle 3:f282664610ba 244 // WE ARE READING STUFFF HERE //
jjohnle 4:7d027ceba0a0 245 // if(can2.read(msg) && msg.id == (MC_BASE+MC_VEL)) {
jjohnle 4:7d027ceba0a0 246 // for (int i = 0; i < msg.len; i++) {
jjohnle 4:7d027ceba0a0 247 // rdata2[i] = msg.data[i];
jjohnle 4:7d027ceba0a0 248 // }
jjohnle 4:7d027ceba0a0 249 //
armin1376 6:e7c29e46ad6c 250 // sending value to CAR can
armin1376 6:e7c29e46ad6c 251
jjohnle 4:7d027ceba0a0 252 //
jjohnle 4:7d027ceba0a0 253 // }
armin1376 5:f55622eacb87 254
armin1376 5:f55622eacb87 255 if(can1.read(msg2)) {
armin1376 5:f55622eacb87 256 if(msg2.id == DASH_Forward) {
armin1376 5:f55622eacb87 257 for(int i = 0; i < msg2.len; i++) {
armin1376 5:f55622eacb87 258 ddata[i] = msg2.data[i];
armin1376 5:f55622eacb87 259 }
armin1376 5:f55622eacb87 260
armin1376 5:f55622eacb87 261 float ddatafloat = *((float*) &ddata);
armin1376 5:f55622eacb87 262 int ddatabit = ceil(ddatafloat);
armin1376 5:f55622eacb87 263
armin1376 5:f55622eacb87 264
armin1376 5:f55622eacb87 265 if(ddatabit == 2147483647) {
armin1376 5:f55622eacb87 266 forward = 1;
armin1376 5:f55622eacb87 267 } else forward = 0;
armin1376 5:f55622eacb87 268
armin1376 5:f55622eacb87 269 pc.printf("ddatabit: %d", ddatabit);
armin1376 5:f55622eacb87 270 pc.printf("forward: %d \n", forward);
armin1376 5:f55622eacb87 271
armin1376 5:f55622eacb87 272
armin1376 5:f55622eacb87 273 }
armin1376 5:f55622eacb87 274
armin1376 5:f55622eacb87 275 else if(msg2.id == DASH_Cruise_Set) {
armin1376 5:f55622eacb87 276 for(int i = 0; i < msg2.len; i++) {
armin1376 5:f55622eacb87 277 ddata1[i] = msg2.data[i];
armin1376 5:f55622eacb87 278 }
armin1376 5:f55622eacb87 279
armin1376 5:f55622eacb87 280 float ddatafloat1 = *((float*) &ddata1);
armin1376 5:f55622eacb87 281 int ddatabit1 = ceil(ddatafloat1);
armin1376 5:f55622eacb87 282
armin1376 5:f55622eacb87 283 if(ddatabit1 == 2147483647) {
armin1376 5:f55622eacb87 284 cruiseSet = 1;
armin1376 5:f55622eacb87 285 } else cruiseSet = 0;
armin1376 5:f55622eacb87 286
armin1376 5:f55622eacb87 287 pc.printf("ddatabit1: %d", ddatabit1);
armin1376 5:f55622eacb87 288 pc.printf("cruise set: %d \n", cruiseSet);
armin1376 5:f55622eacb87 289
armin1376 5:f55622eacb87 290
armin1376 5:f55622eacb87 291 }
armin1376 5:f55622eacb87 292
armin1376 5:f55622eacb87 293 else if(msg2.id == DASH_Cruise) {
armin1376 5:f55622eacb87 294 for(int i = 0; i < msg2.len; i++) {
armin1376 5:f55622eacb87 295 ddata2[i] = msg2.data[i];
armin1376 5:f55622eacb87 296 }
armin1376 5:f55622eacb87 297
armin1376 5:f55622eacb87 298 float ddatafloat2 = *((float*) &ddata2);
armin1376 5:f55622eacb87 299 int ddatabit2 = ceil(ddatafloat2);
armin1376 5:f55622eacb87 300
armin1376 5:f55622eacb87 301 if(ddatabit2 == 2147483647) {
armin1376 5:f55622eacb87 302 cruise = 1;
armin1376 5:f55622eacb87 303 } else cruise = 0;
armin1376 5:f55622eacb87 304
armin1376 5:f55622eacb87 305 pc.printf("ddatabit2: %d", ddatabit2);
armin1376 5:f55622eacb87 306 pc.printf("cruise: %d \n", cruise);
armin1376 5:f55622eacb87 307
armin1376 5:f55622eacb87 308 }
armin1376 5:f55622eacb87 309
armin1376 5:f55622eacb87 310 else if(msg2.id == DASH_Regen) {
armin1376 5:f55622eacb87 311 for(int i = 0; i < msg2.len; i++) {
armin1376 5:f55622eacb87 312 ddata3[i] = msg2.data[i];
armin1376 5:f55622eacb87 313 }
armin1376 5:f55622eacb87 314 pc.printf("regen ddata: %f", ddata3);
armin1376 5:f55622eacb87 315
armin1376 5:f55622eacb87 316 urxdata.rcvdata[3] = ddata3[3];
armin1376 5:f55622eacb87 317 urxdata.rcvdata[2] = ddata3[2];
armin1376 5:f55622eacb87 318 urxdata.rcvdata[1] = ddata3[1];
armin1376 5:f55622eacb87 319 urxdata.rcvdata[0] = ddata3[0];
armin1376 5:f55622eacb87 320 regen = urxdata.rxdata;
armin1376 5:f55622eacb87 321
armin1376 5:f55622eacb87 322 pc.printf("here in regen \n");
armin1376 5:f55622eacb87 323 pc.printf("%f", regen);
armin1376 5:f55622eacb87 324
armin1376 5:f55622eacb87 325 }
armin1376 5:f55622eacb87 326 }
armin1376 5:f55622eacb87 327
jjohnle 4:7d027ceba0a0 328 if(can2.read(msg)){
jjohnle 4:7d027ceba0a0 329 if(msg.id == id3 ) { // Tritium Bus
jjohnle 3:f282664610ba 330 for(int i = 0; i < msg.len; i++) {
jjohnle 3:f282664610ba 331 rdata[i] = msg.data[i];
jjohnle 0:c61d18b01f8c 332 }
jjohnle 4:7d027ceba0a0 333
jjohnle 3:f282664610ba 334 // sending value to CAR can
jjohnle 3:f282664610ba 335 if (!can1.write(CANMessage(0x242, (char*) rdata, 8))) {
jjohnle 3:f282664610ba 336 pc.printf("Cannot write to CAN\n");
jjohnle 3:f282664610ba 337 }
armin1376 7:3352a96997b3 338 wait(0.1);
jjohnle 4:7d027ceba0a0 339
jjohnle 3:f282664610ba 340 urxdata.rcvdata[3] = rdata[7];
jjohnle 3:f282664610ba 341 urxdata.rcvdata[2] = rdata[6];
jjohnle 3:f282664610ba 342 urxdata.rcvdata[1] = rdata[5];
jjohnle 3:f282664610ba 343 urxdata.rcvdata[0] = rdata[4];
jjohnle 3:f282664610ba 344 DCbuscur = urxdata.rxdata;
jjohnle 3:f282664610ba 345 urxdata.rcvdata[3] = rdata[3];
jjohnle 3:f282664610ba 346 urxdata.rcvdata[2] = rdata[2];
jjohnle 3:f282664610ba 347 urxdata.rcvdata[1] = rdata[1];
jjohnle 3:f282664610ba 348 urxdata.rcvdata[0] = rdata[0];
jjohnle 3:f282664610ba 349 DCbusvolt = urxdata.rxdata;
jjohnle 3:f282664610ba 350 }
jjohnle 4:7d027ceba0a0 351
jjohnle 4:7d027ceba0a0 352
jjohnle 3:f282664610ba 353 // reading vehicle and motor velocity
armin1376 6:e7c29e46ad6c 354
jjohnle 4:7d027ceba0a0 355
armin1376 5:f55622eacb87 356 // // reading phase currents
armin1376 5:f55622eacb87 357 // else if(msg.id == (MC_BASE+MC_PHCUR)) {
armin1376 5:f55622eacb87 358 // for (int i = 0; i < msg.len; i++) {
armin1376 5:f55622eacb87 359 // rdata3[i] = msg.data[i];
armin1376 5:f55622eacb87 360 // }
armin1376 5:f55622eacb87 361 //
armin1376 5:f55622eacb87 362 // // sending value to CAR can
armin1376 5:f55622eacb87 363 // if (!can1.write(CANMessage(0x244, (char*) rdata3, 8))) {
armin1376 5:f55622eacb87 364 // pc.printf("Cannot write to CAN\n");
armin1376 5:f55622eacb87 365 // }
armin1376 5:f55622eacb87 366 //// wait(0.001);
armin1376 5:f55622eacb87 367 //
armin1376 5:f55622eacb87 368 // urxdata.rcvdata[3] = rdata3[7];
armin1376 5:f55622eacb87 369 // urxdata.rcvdata[2] = rdata3[6];
armin1376 5:f55622eacb87 370 // urxdata.rcvdata[1] = rdata3[5];
armin1376 5:f55622eacb87 371 // urxdata.rcvdata[0] = rdata3[4];
armin1376 5:f55622eacb87 372 // phaseCcurrent = urxdata.rxdata;
armin1376 5:f55622eacb87 373 // urxdata.rcvdata[3] = rdata3[3];
armin1376 5:f55622eacb87 374 // urxdata.rcvdata[2] = rdata3[2];
armin1376 5:f55622eacb87 375 // urxdata.rcvdata[1] = rdata3[1];
armin1376 5:f55622eacb87 376 // urxdata.rcvdata[0] = rdata3[0];
armin1376 5:f55622eacb87 377 // phaseBcurrent = urxdata.rxdata;
armin1376 5:f55622eacb87 378 // }
jjohnle 4:7d027ceba0a0 379
armin1376 5:f55622eacb87 380 // // reading motor voltage vector
armin1376 5:f55622eacb87 381 // else if(msg.id == (MC_BASE+MC_VOVEC)) {
armin1376 5:f55622eacb87 382 // for (int i = 0; i < msg.len; i++) {
armin1376 5:f55622eacb87 383 // rdata4[i] = msg.data[i];
armin1376 5:f55622eacb87 384 // }
armin1376 5:f55622eacb87 385 //
armin1376 5:f55622eacb87 386 // // sending value to CAR can
armin1376 5:f55622eacb87 387 // if (!can1.write(CANMessage(0x245, (char*) rdata4, 8))) {
armin1376 5:f55622eacb87 388 // pc.printf("Cannot write to CAN\n");
armin1376 5:f55622eacb87 389 // }
armin1376 5:f55622eacb87 390 //// wait(0.001);
armin1376 5:f55622eacb87 391 //
armin1376 5:f55622eacb87 392 // urxdata.rcvdata[3] = rdata4[7];
armin1376 5:f55622eacb87 393 // urxdata.rcvdata[2] = rdata4[6];
armin1376 5:f55622eacb87 394 // urxdata.rcvdata[1] = rdata4[5];
armin1376 5:f55622eacb87 395 // urxdata.rcvdata[0] = rdata4[4];
armin1376 5:f55622eacb87 396 // vd = urxdata.rxdata;
armin1376 5:f55622eacb87 397 // urxdata.rcvdata[3] = rdata4[3];
armin1376 5:f55622eacb87 398 // urxdata.rcvdata[2] = rdata4[2];
armin1376 5:f55622eacb87 399 // urxdata.rcvdata[1] = rdata4[1];
armin1376 5:f55622eacb87 400 // urxdata.rcvdata[0] = rdata4[0];
armin1376 5:f55622eacb87 401 // vq = urxdata.rxdata;
armin1376 5:f55622eacb87 402 // }
armin1376 5:f55622eacb87 403 //
armin1376 5:f55622eacb87 404 // // reading current vector
armin1376 5:f55622eacb87 405 // else if(msg.id == (MC_BASE+MC_CUVEC)) {
armin1376 5:f55622eacb87 406 // for (int i = 0; i < msg.len; i++) {
armin1376 5:f55622eacb87 407 // rdata5[i] = msg.data[i];
armin1376 5:f55622eacb87 408 // }
armin1376 5:f55622eacb87 409 //
armin1376 5:f55622eacb87 410 // // sending value to CAR can
armin1376 5:f55622eacb87 411 // if (!can1.write(CANMessage(0x246, (char*) rdata5, 8))) {
armin1376 5:f55622eacb87 412 // pc.printf("Cannot write to CAN\n");
armin1376 5:f55622eacb87 413 // }
armin1376 5:f55622eacb87 414 //// wait(0.001);
armin1376 5:f55622eacb87 415 //
armin1376 5:f55622eacb87 416 // urxdata.rcvdata[3] = rdata5[7];
armin1376 5:f55622eacb87 417 // urxdata.rcvdata[2] = rdata5[6];
armin1376 5:f55622eacb87 418 // urxdata.rcvdata[1] = rdata5[5];
armin1376 5:f55622eacb87 419 // urxdata.rcvdata[0] = rdata5[4];
armin1376 5:f55622eacb87 420 // Id = urxdata.rxdata;
armin1376 5:f55622eacb87 421 // urxdata.rcvdata[3] = rdata5[3];
armin1376 5:f55622eacb87 422 // urxdata.rcvdata[2] = rdata5[2];
armin1376 5:f55622eacb87 423 // urxdata.rcvdata[1] = rdata5[1];
armin1376 5:f55622eacb87 424 // urxdata.rcvdata[0] = rdata5[0];
armin1376 5:f55622eacb87 425 // Iq = urxdata.rxdata;
armin1376 5:f55622eacb87 426 // }
armin1376 5:f55622eacb87 427 //
armin1376 5:f55622eacb87 428 // // reading back emf
armin1376 5:f55622eacb87 429 // else if(msg.id == (MC_BASE+MC_BEMF)) {
armin1376 5:f55622eacb87 430 // for (int i = 0; i < msg.len; i++) {
armin1376 5:f55622eacb87 431 // rdata6[i] = msg.data[i];
armin1376 5:f55622eacb87 432 // }
armin1376 5:f55622eacb87 433 //
armin1376 5:f55622eacb87 434 // // sending value to CAR can
armin1376 5:f55622eacb87 435 // if (!can1.write(CANMessage(0x247, (char*) rdata6, 8))) {
armin1376 5:f55622eacb87 436 // pc.printf("Cannot write to CAN\n");
armin1376 5:f55622eacb87 437 // }
armin1376 5:f55622eacb87 438 //// wait(0.001);
armin1376 5:f55622eacb87 439 //
armin1376 5:f55622eacb87 440 // urxdata.rcvdata[3] = rdata6[7];
armin1376 5:f55622eacb87 441 // urxdata.rcvdata[2] = rdata6[6];
armin1376 5:f55622eacb87 442 // urxdata.rcvdata[1] = rdata6[5];
armin1376 5:f55622eacb87 443 // urxdata.rcvdata[0] = rdata6[4];
armin1376 5:f55622eacb87 444 // BEMFd = urxdata.rxdata;
armin1376 5:f55622eacb87 445 // urxdata.rcvdata[3] = rdata6[3];
armin1376 5:f55622eacb87 446 // urxdata.rcvdata[2] = rdata6[2];
armin1376 5:f55622eacb87 447 // urxdata.rcvdata[1] = rdata6[1];
armin1376 5:f55622eacb87 448 // urxdata.rcvdata[0] = rdata6[0];
armin1376 5:f55622eacb87 449 // BEMFq = urxdata.rxdata;
armin1376 5:f55622eacb87 450 // }
jjohnle 4:7d027ceba0a0 451
jjohnle 3:f282664610ba 452 // reading heatsink and motor temp
jjohnle 4:7d027ceba0a0 453 else if(msg.id == (MC_BASE+MC_TEMP)) {
jjohnle 3:f282664610ba 454 for (int i = 0; i < msg.len; i++) {
jjohnle 3:f282664610ba 455 rdata7[i] = msg.data[i];
jjohnle 2:2a4822c7c91a 456 }
jjohnle 4:7d027ceba0a0 457
jjohnle 3:f282664610ba 458 // sending value to CAR can
jjohnle 3:f282664610ba 459 if (!can1.write(CANMessage(0x24B, (char*) rdata7, 8))) {
jjohnle 3:f282664610ba 460 pc.printf("Cannot write to CAN\n");
jjohnle 2:2a4822c7c91a 461 }
armin1376 7:3352a96997b3 462 wait(0.1);
jjohnle 4:7d027ceba0a0 463
jjohnle 3:f282664610ba 464 urxdata.rcvdata[3] = rdata7[7];
jjohnle 3:f282664610ba 465 urxdata.rcvdata[2] = rdata7[6];
jjohnle 3:f282664610ba 466 urxdata.rcvdata[1] = rdata7[5];
jjohnle 3:f282664610ba 467 urxdata.rcvdata[0] = rdata7[4];
jjohnle 3:f282664610ba 468 heatSinkTemp = urxdata.rxdata;
jjohnle 3:f282664610ba 469 urxdata.rcvdata[3] = rdata7[3];
jjohnle 3:f282664610ba 470 urxdata.rcvdata[2] = rdata7[2];
jjohnle 3:f282664610ba 471 urxdata.rcvdata[1] = rdata7[1];
jjohnle 3:f282664610ba 472 urxdata.rcvdata[0] = rdata7[0];
jjohnle 3:f282664610ba 473 motorTemp = urxdata.rxdata;
jjohnle 3:f282664610ba 474 }
jjohnle 4:7d027ceba0a0 475
armin1376 5:f55622eacb87 476 // // reading odometer and bus amp ohours measuremeant
armin1376 5:f55622eacb87 477 // else if(msg.id == (MC_BASE+MC_AMPH)) {
armin1376 5:f55622eacb87 478 // for (int i = 0; i < msg.len; i++) {
armin1376 5:f55622eacb87 479 // rdata8[i] = msg.data[i];
armin1376 5:f55622eacb87 480 // }
armin1376 5:f55622eacb87 481 //
armin1376 5:f55622eacb87 482 // // sending value to CAR can
armin1376 5:f55622eacb87 483 // if (!can1.write(CANMessage(0x24E, (char*) rdata8, 8))) {
armin1376 5:f55622eacb87 484 // pc.printf("Cannot write to CAN\n");
armin1376 5:f55622eacb87 485 // }
armin1376 5:f55622eacb87 486 //// wait(0.001);
armin1376 5:f55622eacb87 487 //
armin1376 5:f55622eacb87 488 // urxdata.rcvdata[3] = rdata8[7];
armin1376 5:f55622eacb87 489 // urxdata.rcvdata[2] = rdata8[6];
armin1376 5:f55622eacb87 490 // urxdata.rcvdata[1] = rdata8[5];
armin1376 5:f55622eacb87 491 // urxdata.rcvdata[0] = rdata8[4];
armin1376 5:f55622eacb87 492 // DCBusAmpHours = urxdata.rxdata;
armin1376 5:f55622eacb87 493 // urxdata.rcvdata[3] = rdata8[3];
armin1376 5:f55622eacb87 494 // urxdata.rcvdata[2] = rdata8[2];
armin1376 5:f55622eacb87 495 // urxdata.rcvdata[1] = rdata8[1];
armin1376 5:f55622eacb87 496 // urxdata.rcvdata[0] = rdata8[0];
armin1376 5:f55622eacb87 497 // odometerValue = urxdata.rxdata;
armin1376 5:f55622eacb87 498 // }
jjohnle 4:7d027ceba0a0 499 }
jjohnle 4:7d027ceba0a0 500
jjohnle 3:f282664610ba 501 if(alive % 100 == 0){
jjohnle 3:f282664610ba 502 printf("Motor board is running \n\r");
jjohnle 3:f282664610ba 503 printf(" Requested Motor Current: %f\n\r", current);
jjohnle 3:f282664610ba 504 printf(" Requested Motor Velocity: %f\n\r", velocity);
jjohnle 3:f282664610ba 505 printf(" DC Bus Current (A) = %f",DCbuscur);
jjohnle 3:f282664610ba 506 printf("\r\n");
jjohnle 3:f282664610ba 507 printf(" DC Bus Voltage (V) = %f",DCbusvolt);
jjohnle 3:f282664610ba 508 printf("\r\n");
jjohnle 4:7d027ceba0a0 509
jjohnle 3:f282664610ba 510 // Printing other values
jjohnle 3:f282664610ba 511 printf(" Vehicle Velocity (RPM) = %f",vehicleVel);
jjohnle 3:f282664610ba 512 printf("\r\n");
jjohnle 3:f282664610ba 513 printf(" Motor Velocity (V) = %f",motorVel);
jjohnle 3:f282664610ba 514 printf("\r\n");
armin1376 5:f55622eacb87 515 // printf(" Phase B Current (A-rms) = %f",phaseBcurrent);
armin1376 5:f55622eacb87 516 // printf("\r\n");
armin1376 5:f55622eacb87 517 // printf(" Phase C Current (A-rms) = %f",phaseCcurrent);
armin1376 5:f55622eacb87 518 // printf("\r\n");
armin1376 5:f55622eacb87 519 // printf(" Vd (V) = %f",vd);
armin1376 5:f55622eacb87 520 // printf("\r\n");
armin1376 5:f55622eacb87 521 // printf(" Vq (V) = %f",vq);
armin1376 5:f55622eacb87 522 // printf("\r\n");
armin1376 5:f55622eacb87 523 //
armin1376 5:f55622eacb87 524 // printf(" Id (A) = %f",Id);
armin1376 5:f55622eacb87 525 // printf("\r\n");
armin1376 5:f55622eacb87 526 // printf(" Iq (A) = %f",Iq);
armin1376 5:f55622eacb87 527 // printf("\r\n");
armin1376 5:f55622eacb87 528 // printf(" BEMFd (V) = %f",BEMFd);
armin1376 5:f55622eacb87 529 // printf("\r\n");
armin1376 5:f55622eacb87 530 // printf(" BEMFq (V) = %f",BEMFq);
armin1376 5:f55622eacb87 531 // printf("\r\n");
jjohnle 3:f282664610ba 532 printf(" Heat Sink Temperature (Celsius) = %f",heatSinkTemp);
jjohnle 3:f282664610ba 533 printf("\r\n");
jjohnle 3:f282664610ba 534 printf(" Motor Temperature (Celsius) = %f",motorTemp);
jjohnle 3:f282664610ba 535 printf("\r\n");
armin1376 5:f55622eacb87 536 // printf(" DC Bus (Ah) = %f",DCBusAmpHours);
armin1376 5:f55622eacb87 537 // printf("\r\n");
armin1376 5:f55622eacb87 538 // printf(" Odometer (Distance) (m) = %f",odometerValue);
armin1376 5:f55622eacb87 539 // printf("\r\n");
armin1376 5:f55622eacb87 540 //
jjohnle 4:7d027ceba0a0 541
jjohnle 3:f282664610ba 542 // blinking LED
jjohnle 3:f282664610ba 543 LED8 = !LED8;
jjohnle 3:f282664610ba 544 if (!can1.write(CANMessage(id, (char*)data, 8))) //send current and velocity over car CAN
jjohnle 3:f282664610ba 545 printf("Car CAN failed \n\r");
jjohnle 3:f282664610ba 546 }
jjohnle 3:f282664610ba 547 alive++;
jjohnle 3:f282664610ba 548 }
jjohnle 3:f282664610ba 549 }