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

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@4:7d027ceba0a0, 2019-11-09 (annotated)

Committer:
jjohnle
Date:
Sat Nov 09 20:30:32 2019 +0000
Revision:
4:7d027ceba0a0
Parent:
3:f282664610ba
Child:
5:f55622eacb87
works and fast

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
jjohnle 4:7d027ceba0a0 42
jjohnle 3:f282664610ba 43 CAN can1(PD_0,PD_1,125000); // can1 is car CAN (Rx, Tx, speed)
jjohnle 3:f282664610ba 44 CAN can2(PB_5,PB_6,50000); // can2 is motor controller CAN (Rx, Tx, speed)
jjohnle 0:c61d18b01f8c 45 Serial pc(USBTX, USBRX);
jjohnle 4:7d027ceba0a0 46
jjohnle 3:f282664610ba 47 AnalogIn ain(PB_0);
jjohnle 4:7d027ceba0a0 48
jjohnle 3:f282664610ba 49 DigitalOut LED8(PF_2);
jjohnle 3:f282664610ba 50 DigitalOut LED7(PA_7);
jjohnle 3:f282664610ba 51 DigitalOut LED6(PF_10);
jjohnle 3:f282664610ba 52 DigitalOut LED5(PF_5);
jjohnle 3:f282664610ba 53 DigitalOut LED4a(PF_3);
jjohnle 3:f282664610ba 54 DigitalOut LED3a(PC_3);
jjohnle 3:f282664610ba 55 DigitalOut LED2a(PC_0);
jjohnle 3:f282664610ba 56 DigitalOut LED1a(PA_3);
jjohnle 4:7d027ceba0a0 57
jjohnle 3:f282664610ba 58 #define MAX_VELOCITY 100 // motor velocity in m/s
jjohnle 3:f282664610ba 59 #define MAX_CURRENT 1.0 // desired motor current as percentage of max current
jjohnle 4:7d027ceba0a0 60
jjohnle 3:f282664610ba 61 int main() {
jjohnle 3:f282664610ba 62 float current = MAX_CURRENT;
jjohnle 3:f282664610ba 63 float velocity = MAX_VELOCITY;
jjohnle 3:f282664610ba 64 float bus_current = MAX_CURRENT;
jjohnle 3:f282664610ba 65 float DCbuscur;
jjohnle 3:f282664610ba 66 float DCbusvolt;
jjohnle 3:f282664610ba 67 double pedal_position;
jjohnle 3:f282664610ba 68 float data[2];
jjohnle 3:f282664610ba 69 float data2[2];
jjohnle 3:f282664610ba 70 float meas;
jjohnle 3:f282664610ba 71 double avgval;
jjohnle 3:f282664610ba 72 int n;
jjohnle 3:f282664610ba 73 int dummy;
jjohnle 3:f282664610ba 74 int alive;
jjohnle 4:7d027ceba0a0 75
jjohnle 3:f282664610ba 76 // other ids we need to read
jjohnle 3:f282664610ba 77 float phaseBcurrent;
jjohnle 3:f282664610ba 78 float phaseCcurrent;
jjohnle 3:f282664610ba 79 float vehicleVel;
jjohnle 3:f282664610ba 80 float motorVel;
jjohnle 3:f282664610ba 81 float vd;
jjohnle 3:f282664610ba 82 float vq;
jjohnle 3:f282664610ba 83 float Id;
jjohnle 3:f282664610ba 84 float Iq;
jjohnle 3:f282664610ba 85 float BEMFd;
jjohnle 3:f282664610ba 86 float BEMFq;
jjohnle 3:f282664610ba 87 float heatSinkTemp;
jjohnle 3:f282664610ba 88 float motorTemp;
jjohnle 3:f282664610ba 89 float DCBusAmpHours;
jjohnle 3:f282664610ba 90 float odometerValue;
jjohnle 4:7d027ceba0a0 91
jjohnle 3:f282664610ba 92 //char const * serial = "0002173";
jjohnle 3:f282664610ba 93 // can1.frequency(500000);
jjohnle 3:f282664610ba 94 int id;
jjohnle 3:f282664610ba 95 int id2;
jjohnle 3:f282664610ba 96 int id3;
jjohnle 3:f282664610ba 97 char rdata[8];
jjohnle 3:f282664610ba 98 char rdata2[8];
jjohnle 3:f282664610ba 99 char rdata3[8];
jjohnle 3:f282664610ba 100 char rdata4[8];
jjohnle 3:f282664610ba 101 char rdata5[8];
jjohnle 3:f282664610ba 102 char rdata6[8];
jjohnle 3:f282664610ba 103 char rdata7[8];
jjohnle 3:f282664610ba 104 char rdata8[8];
jjohnle 3:f282664610ba 105 char rdata9[8];
jjohnle 4:7d027ceba0a0 106
jjohnle 3:f282664610ba 107 CANMessage msg;
jjohnle 3:f282664610ba 108 dummy = 0;
jjohnle 3:f282664610ba 109 alive = 0;
jjohnle 4:7d027ceba0a0 110
jjohnle 3:f282664610ba 111 union {
jjohnle 3:f282664610ba 112 char rcvdata[4];
jjohnle 3:f282664610ba 113 float rxdata;
jjohnle 3:f282664610ba 114 } urxdata;
jjohnle 4:7d027ceba0a0 115
jjohnle 3:f282664610ba 116 id = DC_BASE + DC_DRIVE;
jjohnle 3:f282664610ba 117 id2 = DC_BASE + DC_POWER;
jjohnle 3:f282664610ba 118 id3 = MC_BASE + DC_POWER;
jjohnle 4:7d027ceba0a0 119
jjohnle 3:f282664610ba 120 while (1) {
jjohnle 3:f282664610ba 121 n = 0;
jjohnle 3:f282664610ba 122 avgval = 0.0;
jjohnle 3:f282664610ba 123 while(n < 100) {
jjohnle 3:f282664610ba 124 meas = ain.read();
jjohnle 3:f282664610ba 125 avgval = avgval + meas;
jjohnle 3:f282664610ba 126 n++ ;
jjohnle 2:2a4822c7c91a 127 }
jjohnle 3:f282664610ba 128 pedal_position = avgval/100.0;
jjohnle 4:7d027ceba0a0 129
jjohnle 3:f282664610ba 130 if(pedal_position > 0.005)
jjohnle 3:f282664610ba 131 LED1a = 1;
jjohnle 3:f282664610ba 132 else
jjohnle 3:f282664610ba 133 LED1a = 0;
jjohnle 3:f282664610ba 134 if(pedal_position > 0.01)
jjohnle 3:f282664610ba 135 LED2a = 1;
jjohnle 3:f282664610ba 136 else
jjohnle 3:f282664610ba 137 LED2a = 0;
jjohnle 3:f282664610ba 138 if(pedal_position > 0.015)
jjohnle 3:f282664610ba 139 LED3a = 1;
jjohnle 3:f282664610ba 140 else
jjohnle 3:f282664610ba 141 LED3a = 0;
jjohnle 3:f282664610ba 142 if(pedal_position > 0.02)
jjohnle 3:f282664610ba 143 LED4a = 1;
jjohnle 3:f282664610ba 144 else
jjohnle 3:f282664610ba 145 LED4a = 0;
jjohnle 3:f282664610ba 146 if(pedal_position > 0.025)
jjohnle 3:f282664610ba 147 LED5 = 1;
jjohnle 3:f282664610ba 148 else
jjohnle 3:f282664610ba 149 LED5 = 0;
jjohnle 3:f282664610ba 150 if(pedal_position > 0.03)
jjohnle 3:f282664610ba 151 LED6 = 1;
jjohnle 3:f282664610ba 152 else
jjohnle 3:f282664610ba 153 LED6 = 0;
jjohnle 3:f282664610ba 154 if(pedal_position > 0.035)
jjohnle 3:f282664610ba 155 LED7 = 1;
jjohnle 3:f282664610ba 156 else
jjohnle 3:f282664610ba 157 LED7 = 0;
jjohnle 4:7d027ceba0a0 158
jjohnle 3:f282664610ba 159 current = MAX_CURRENT * pedal_position;
jjohnle 3:f282664610ba 160 velocity = 9.0;
jjohnle 4:7d027ceba0a0 161
jjohnle 3:f282664610ba 162 data[1] = current; // Flipped because of endianness
jjohnle 3:f282664610ba 163 data[0] = velocity;
jjohnle 4:7d027ceba0a0 164
jjohnle 3:f282664610ba 165 if (!can2.write(CANMessage(id, (char*)data, 8))) // send current and velocity to Tritum
jjohnle 3:f282664610ba 166 printf("Drive failed \n\r");
jjohnle 4:7d027ceba0a0 167
jjohnle 3:f282664610ba 168 data2[1] = bus_current;
jjohnle 3:f282664610ba 169 data2[0] = 0.0;
jjohnle 3:f282664610ba 170 if (!can2.write(CANMessage(id2, (char*)data2, 8)))
jjohnle 3:f282664610ba 171 dummy = 0;
jjohnle 4:7d027ceba0a0 172
jjohnle 3:f282664610ba 173 wait_ms(10); // Need message every 250ms to maintain operation
jjohnle 4:7d027ceba0a0 174
jjohnle 3:f282664610ba 175 // WE ARE READING STUFFF HERE //
jjohnle 4:7d027ceba0a0 176 // if(can2.read(msg) && msg.id == (MC_BASE+MC_VEL)) {
jjohnle 4:7d027ceba0a0 177 // for (int i = 0; i < msg.len; i++) {
jjohnle 4:7d027ceba0a0 178 // rdata2[i] = msg.data[i];
jjohnle 4:7d027ceba0a0 179 // }
jjohnle 4:7d027ceba0a0 180 //
jjohnle 4:7d027ceba0a0 181 // // sending value to CAR can
jjohnle 4:7d027ceba0a0 182 // if (!can1.write(CANMessage(0x243, (char*) rdata2, 8))) {
jjohnle 4:7d027ceba0a0 183 // pc.printf("Cannot write to CAN\n");
jjohnle 4:7d027ceba0a0 184 // }
jjohnle 4:7d027ceba0a0 185 // wait(0.01);
jjohnle 4:7d027ceba0a0 186 //
jjohnle 4:7d027ceba0a0 187 // urxdata.rcvdata[3] = rdata2[7];
jjohnle 4:7d027ceba0a0 188 // urxdata.rcvdata[2] = rdata2[6];
jjohnle 4:7d027ceba0a0 189 // urxdata.rcvdata[1] = rdata2[5];
jjohnle 4:7d027ceba0a0 190 // urxdata.rcvdata[0] = rdata2[4];
jjohnle 4:7d027ceba0a0 191 // vehicleVel = urxdata.rxdata;
jjohnle 4:7d027ceba0a0 192 // urxdata.rcvdata[3] = rdata2[3];
jjohnle 4:7d027ceba0a0 193 // urxdata.rcvdata[2] = rdata2[2];
jjohnle 4:7d027ceba0a0 194 // urxdata.rcvdata[1] = rdata2[1];
jjohnle 4:7d027ceba0a0 195 // urxdata.rcvdata[0] = rdata2[0];
jjohnle 4:7d027ceba0a0 196 // motorVel = urxdata.rxdata;
jjohnle 4:7d027ceba0a0 197 // }
jjohnle 4:7d027ceba0a0 198 //
jjohnle 4:7d027ceba0a0 199 // }
jjohnle 4:7d027ceba0a0 200
jjohnle 4:7d027ceba0a0 201 if(can2.read(msg)){
jjohnle 4:7d027ceba0a0 202 if(msg.id == id3 ) { // Tritium Bus
jjohnle 3:f282664610ba 203 for(int i = 0; i < msg.len; i++) {
jjohnle 3:f282664610ba 204 rdata[i] = msg.data[i];
jjohnle 0:c61d18b01f8c 205 }
jjohnle 4:7d027ceba0a0 206
jjohnle 3:f282664610ba 207 // sending value to CAR can
jjohnle 3:f282664610ba 208 if (!can1.write(CANMessage(0x242, (char*) rdata, 8))) {
jjohnle 3:f282664610ba 209 pc.printf("Cannot write to CAN\n");
jjohnle 3:f282664610ba 210 }
jjohnle 4:7d027ceba0a0 211 wait(0.001);
jjohnle 4:7d027ceba0a0 212
jjohnle 3:f282664610ba 213 urxdata.rcvdata[3] = rdata[7];
jjohnle 3:f282664610ba 214 urxdata.rcvdata[2] = rdata[6];
jjohnle 3:f282664610ba 215 urxdata.rcvdata[1] = rdata[5];
jjohnle 3:f282664610ba 216 urxdata.rcvdata[0] = rdata[4];
jjohnle 3:f282664610ba 217 DCbuscur = urxdata.rxdata;
jjohnle 3:f282664610ba 218 urxdata.rcvdata[3] = rdata[3];
jjohnle 3:f282664610ba 219 urxdata.rcvdata[2] = rdata[2];
jjohnle 3:f282664610ba 220 urxdata.rcvdata[1] = rdata[1];
jjohnle 3:f282664610ba 221 urxdata.rcvdata[0] = rdata[0];
jjohnle 3:f282664610ba 222 DCbusvolt = urxdata.rxdata;
jjohnle 3:f282664610ba 223 }
jjohnle 4:7d027ceba0a0 224
jjohnle 4:7d027ceba0a0 225
jjohnle 3:f282664610ba 226 // reading vehicle and motor velocity
jjohnle 4:7d027ceba0a0 227 else if(msg.id == (MC_BASE+MC_VEL)) {
jjohnle 3:f282664610ba 228 for (int i = 0; i < msg.len; i++) {
jjohnle 3:f282664610ba 229 rdata2[i] = msg.data[i];
jjohnle 3:f282664610ba 230 }
jjohnle 4:7d027ceba0a0 231
jjohnle 3:f282664610ba 232 // sending value to CAR can
jjohnle 3:f282664610ba 233 if (!can1.write(CANMessage(0x243, (char*) rdata2, 8))) {
jjohnle 3:f282664610ba 234 pc.printf("Cannot write to CAN\n");
jjohnle 3:f282664610ba 235 }
jjohnle 4:7d027ceba0a0 236 wait(0.001);
jjohnle 4:7d027ceba0a0 237
jjohnle 3:f282664610ba 238 urxdata.rcvdata[3] = rdata2[7];
jjohnle 3:f282664610ba 239 urxdata.rcvdata[2] = rdata2[6];
jjohnle 3:f282664610ba 240 urxdata.rcvdata[1] = rdata2[5];
jjohnle 3:f282664610ba 241 urxdata.rcvdata[0] = rdata2[4];
jjohnle 3:f282664610ba 242 vehicleVel = urxdata.rxdata;
jjohnle 3:f282664610ba 243 urxdata.rcvdata[3] = rdata2[3];
jjohnle 3:f282664610ba 244 urxdata.rcvdata[2] = rdata2[2];
jjohnle 3:f282664610ba 245 urxdata.rcvdata[1] = rdata2[1];
jjohnle 3:f282664610ba 246 urxdata.rcvdata[0] = rdata2[0];
jjohnle 3:f282664610ba 247 motorVel = urxdata.rxdata;
jjohnle 3:f282664610ba 248 }
jjohnle 4:7d027ceba0a0 249
jjohnle 0:c61d18b01f8c 250 // reading phase currents
jjohnle 4:7d027ceba0a0 251 else if(msg.id == (MC_BASE+MC_PHCUR)) {
jjohnle 3:f282664610ba 252 for (int i = 0; i < msg.len; i++) {
jjohnle 3:f282664610ba 253 rdata3[i] = msg.data[i];
jjohnle 3:f282664610ba 254 }
jjohnle 4:7d027ceba0a0 255
jjohnle 3:f282664610ba 256 // sending value to CAR can
jjohnle 3:f282664610ba 257 if (!can1.write(CANMessage(0x244, (char*) rdata3, 8))) {
jjohnle 3:f282664610ba 258 pc.printf("Cannot write to CAN\n");
jjohnle 3:f282664610ba 259 }
jjohnle 4:7d027ceba0a0 260 wait(0.001);
jjohnle 4:7d027ceba0a0 261
jjohnle 3:f282664610ba 262 urxdata.rcvdata[3] = rdata3[7];
jjohnle 3:f282664610ba 263 urxdata.rcvdata[2] = rdata3[6];
jjohnle 3:f282664610ba 264 urxdata.rcvdata[1] = rdata3[5];
jjohnle 3:f282664610ba 265 urxdata.rcvdata[0] = rdata3[4];
jjohnle 3:f282664610ba 266 phaseCcurrent = urxdata.rxdata;
jjohnle 3:f282664610ba 267 urxdata.rcvdata[3] = rdata3[3];
jjohnle 3:f282664610ba 268 urxdata.rcvdata[2] = rdata3[2];
jjohnle 3:f282664610ba 269 urxdata.rcvdata[1] = rdata3[1];
jjohnle 3:f282664610ba 270 urxdata.rcvdata[0] = rdata3[0];
jjohnle 3:f282664610ba 271 phaseBcurrent = urxdata.rxdata;
jjohnle 0:c61d18b01f8c 272 }
jjohnle 4:7d027ceba0a0 273
jjohnle 0:c61d18b01f8c 274 // reading motor voltage vector
jjohnle 4:7d027ceba0a0 275 else if(msg.id == (MC_BASE+MC_VOVEC)) {
jjohnle 3:f282664610ba 276 for (int i = 0; i < msg.len; i++) {
jjohnle 3:f282664610ba 277 rdata4[i] = msg.data[i];
jjohnle 0:c61d18b01f8c 278 }
jjohnle 4:7d027ceba0a0 279
jjohnle 3:f282664610ba 280 // sending value to CAR can
jjohnle 3:f282664610ba 281 if (!can1.write(CANMessage(0x245, (char*) rdata4, 8))) {
jjohnle 3:f282664610ba 282 pc.printf("Cannot write to CAN\n");
jjohnle 0:c61d18b01f8c 283 }
jjohnle 4:7d027ceba0a0 284 wait(0.001);
jjohnle 4:7d027ceba0a0 285
jjohnle 3:f282664610ba 286 urxdata.rcvdata[3] = rdata4[7];
jjohnle 3:f282664610ba 287 urxdata.rcvdata[2] = rdata4[6];
jjohnle 3:f282664610ba 288 urxdata.rcvdata[1] = rdata4[5];
jjohnle 3:f282664610ba 289 urxdata.rcvdata[0] = rdata4[4];
jjohnle 3:f282664610ba 290 vd = urxdata.rxdata;
jjohnle 3:f282664610ba 291 urxdata.rcvdata[3] = rdata4[3];
jjohnle 3:f282664610ba 292 urxdata.rcvdata[2] = rdata4[2];
jjohnle 3:f282664610ba 293 urxdata.rcvdata[1] = rdata4[1];
jjohnle 3:f282664610ba 294 urxdata.rcvdata[0] = rdata4[0];
jjohnle 3:f282664610ba 295 vq = urxdata.rxdata;
jjohnle 3:f282664610ba 296 }
jjohnle 4:7d027ceba0a0 297
jjohnle 3:f282664610ba 298 // reading current vector
jjohnle 4:7d027ceba0a0 299 else if(msg.id == (MC_BASE+MC_CUVEC)) {
jjohnle 3:f282664610ba 300 for (int i = 0; i < msg.len; i++) {
jjohnle 3:f282664610ba 301 rdata5[i] = msg.data[i];
jjohnle 0:c61d18b01f8c 302 }
jjohnle 4:7d027ceba0a0 303
jjohnle 3:f282664610ba 304 // sending value to CAR can
jjohnle 3:f282664610ba 305 if (!can1.write(CANMessage(0x246, (char*) rdata5, 8))) {
jjohnle 3:f282664610ba 306 pc.printf("Cannot write to CAN\n");
jjohnle 3:f282664610ba 307 }
jjohnle 4:7d027ceba0a0 308 wait(0.001);
jjohnle 4:7d027ceba0a0 309
jjohnle 3:f282664610ba 310 urxdata.rcvdata[3] = rdata5[7];
jjohnle 3:f282664610ba 311 urxdata.rcvdata[2] = rdata5[6];
jjohnle 3:f282664610ba 312 urxdata.rcvdata[1] = rdata5[5];
jjohnle 3:f282664610ba 313 urxdata.rcvdata[0] = rdata5[4];
jjohnle 3:f282664610ba 314 Id = urxdata.rxdata;
jjohnle 3:f282664610ba 315 urxdata.rcvdata[3] = rdata5[3];
jjohnle 3:f282664610ba 316 urxdata.rcvdata[2] = rdata5[2];
jjohnle 3:f282664610ba 317 urxdata.rcvdata[1] = rdata5[1];
jjohnle 3:f282664610ba 318 urxdata.rcvdata[0] = rdata5[0];
jjohnle 3:f282664610ba 319 Iq = urxdata.rxdata;
jjohnle 3:f282664610ba 320 }
jjohnle 4:7d027ceba0a0 321
jjohnle 3:f282664610ba 322 // reading back emf
jjohnle 4:7d027ceba0a0 323 else if(msg.id == (MC_BASE+MC_BEMF)) {
jjohnle 3:f282664610ba 324 for (int i = 0; i < msg.len; i++) {
jjohnle 3:f282664610ba 325 rdata6[i] = msg.data[i];
jjohnle 0:c61d18b01f8c 326 }
jjohnle 4:7d027ceba0a0 327
jjohnle 3:f282664610ba 328 // sending value to CAR can
jjohnle 3:f282664610ba 329 if (!can1.write(CANMessage(0x247, (char*) rdata6, 8))) {
jjohnle 3:f282664610ba 330 pc.printf("Cannot write to CAN\n");
jjohnle 3:f282664610ba 331 }
jjohnle 4:7d027ceba0a0 332 wait(0.001);
jjohnle 4:7d027ceba0a0 333
jjohnle 3:f282664610ba 334 urxdata.rcvdata[3] = rdata6[7];
jjohnle 3:f282664610ba 335 urxdata.rcvdata[2] = rdata6[6];
jjohnle 3:f282664610ba 336 urxdata.rcvdata[1] = rdata6[5];
jjohnle 3:f282664610ba 337 urxdata.rcvdata[0] = rdata6[4];
jjohnle 3:f282664610ba 338 BEMFd = urxdata.rxdata;
jjohnle 3:f282664610ba 339 urxdata.rcvdata[3] = rdata6[3];
jjohnle 3:f282664610ba 340 urxdata.rcvdata[2] = rdata6[2];
jjohnle 3:f282664610ba 341 urxdata.rcvdata[1] = rdata6[1];
jjohnle 3:f282664610ba 342 urxdata.rcvdata[0] = rdata6[0];
jjohnle 3:f282664610ba 343 BEMFq = urxdata.rxdata;
jjohnle 2:2a4822c7c91a 344 }
jjohnle 4:7d027ceba0a0 345
jjohnle 3:f282664610ba 346 // reading heatsink and motor temp
jjohnle 4:7d027ceba0a0 347 else if(msg.id == (MC_BASE+MC_TEMP)) {
jjohnle 3:f282664610ba 348 for (int i = 0; i < msg.len; i++) {
jjohnle 3:f282664610ba 349 rdata7[i] = msg.data[i];
jjohnle 2:2a4822c7c91a 350 }
jjohnle 4:7d027ceba0a0 351
jjohnle 3:f282664610ba 352 // sending value to CAR can
jjohnle 3:f282664610ba 353 if (!can1.write(CANMessage(0x24B, (char*) rdata7, 8))) {
jjohnle 3:f282664610ba 354 pc.printf("Cannot write to CAN\n");
jjohnle 2:2a4822c7c91a 355 }
jjohnle 4:7d027ceba0a0 356 wait(0.001);
jjohnle 4:7d027ceba0a0 357
jjohnle 3:f282664610ba 358 urxdata.rcvdata[3] = rdata7[7];
jjohnle 3:f282664610ba 359 urxdata.rcvdata[2] = rdata7[6];
jjohnle 3:f282664610ba 360 urxdata.rcvdata[1] = rdata7[5];
jjohnle 3:f282664610ba 361 urxdata.rcvdata[0] = rdata7[4];
jjohnle 3:f282664610ba 362 heatSinkTemp = urxdata.rxdata;
jjohnle 3:f282664610ba 363 urxdata.rcvdata[3] = rdata7[3];
jjohnle 3:f282664610ba 364 urxdata.rcvdata[2] = rdata7[2];
jjohnle 3:f282664610ba 365 urxdata.rcvdata[1] = rdata7[1];
jjohnle 3:f282664610ba 366 urxdata.rcvdata[0] = rdata7[0];
jjohnle 3:f282664610ba 367 motorTemp = urxdata.rxdata;
jjohnle 3:f282664610ba 368 }
jjohnle 4:7d027ceba0a0 369
jjohnle 3:f282664610ba 370 // reading odometer and bus amp ohours measuremeant
jjohnle 4:7d027ceba0a0 371 else if(msg.id == (MC_BASE+MC_AMPH)) {
jjohnle 3:f282664610ba 372 for (int i = 0; i < msg.len; i++) {
jjohnle 3:f282664610ba 373 rdata8[i] = msg.data[i];
jjohnle 2:2a4822c7c91a 374 }
jjohnle 4:7d027ceba0a0 375
jjohnle 3:f282664610ba 376 // sending value to CAR can
jjohnle 3:f282664610ba 377 if (!can1.write(CANMessage(0x24E, (char*) rdata8, 8))) {
jjohnle 3:f282664610ba 378 pc.printf("Cannot write to CAN\n");
jjohnle 2:2a4822c7c91a 379 }
jjohnle 4:7d027ceba0a0 380 wait(0.001);
jjohnle 4:7d027ceba0a0 381
jjohnle 3:f282664610ba 382 urxdata.rcvdata[3] = rdata8[7];
jjohnle 3:f282664610ba 383 urxdata.rcvdata[2] = rdata8[6];
jjohnle 3:f282664610ba 384 urxdata.rcvdata[1] = rdata8[5];
jjohnle 3:f282664610ba 385 urxdata.rcvdata[0] = rdata8[4];
jjohnle 3:f282664610ba 386 DCBusAmpHours = urxdata.rxdata;
jjohnle 3:f282664610ba 387 urxdata.rcvdata[3] = rdata8[3];
jjohnle 3:f282664610ba 388 urxdata.rcvdata[2] = rdata8[2];
jjohnle 3:f282664610ba 389 urxdata.rcvdata[1] = rdata8[1];
jjohnle 3:f282664610ba 390 urxdata.rcvdata[0] = rdata8[0];
jjohnle 3:f282664610ba 391 odometerValue = urxdata.rxdata;
jjohnle 2:2a4822c7c91a 392 }
jjohnle 4:7d027ceba0a0 393 }
jjohnle 4:7d027ceba0a0 394
jjohnle 3:f282664610ba 395 if(alive % 100 == 0){
jjohnle 3:f282664610ba 396 printf("Motor board is running \n\r");
jjohnle 3:f282664610ba 397 printf(" Requested Motor Current: %f\n\r", current);
jjohnle 3:f282664610ba 398 printf(" Requested Motor Velocity: %f\n\r", velocity);
jjohnle 3:f282664610ba 399 printf(" DC Bus Current (A) = %f",DCbuscur);
jjohnle 3:f282664610ba 400 printf("\r\n");
jjohnle 3:f282664610ba 401 printf(" DC Bus Voltage (V) = %f",DCbusvolt);
jjohnle 3:f282664610ba 402 printf("\r\n");
jjohnle 4:7d027ceba0a0 403
jjohnle 3:f282664610ba 404 // Printing other values
jjohnle 3:f282664610ba 405 printf(" Vehicle Velocity (RPM) = %f",vehicleVel);
jjohnle 3:f282664610ba 406 printf("\r\n");
jjohnle 3:f282664610ba 407 printf(" Motor Velocity (V) = %f",motorVel);
jjohnle 3:f282664610ba 408 printf("\r\n");
jjohnle 3:f282664610ba 409 printf(" Phase B Current (A-rms) = %f",phaseBcurrent);
jjohnle 3:f282664610ba 410 printf("\r\n");
jjohnle 3:f282664610ba 411 printf(" Phase C Current (A-rms) = %f",phaseCcurrent);
jjohnle 3:f282664610ba 412 printf("\r\n");
jjohnle 3:f282664610ba 413 printf(" Vd (V) = %f",vd);
jjohnle 3:f282664610ba 414 printf("\r\n");
jjohnle 3:f282664610ba 415 printf(" Vq (V) = %f",vq);
jjohnle 3:f282664610ba 416 printf("\r\n");
jjohnle 4:7d027ceba0a0 417
jjohnle 3:f282664610ba 418 printf(" Id (A) = %f",Id);
jjohnle 3:f282664610ba 419 printf("\r\n");
jjohnle 3:f282664610ba 420 printf(" Iq (A) = %f",Iq);
jjohnle 3:f282664610ba 421 printf("\r\n");
jjohnle 3:f282664610ba 422 printf(" BEMFd (V) = %f",BEMFd);
jjohnle 3:f282664610ba 423 printf("\r\n");
jjohnle 3:f282664610ba 424 printf(" BEMFq (V) = %f",BEMFq);
jjohnle 3:f282664610ba 425 printf("\r\n");
jjohnle 3:f282664610ba 426 printf(" Heat Sink Temperature (Celsius) = %f",heatSinkTemp);
jjohnle 3:f282664610ba 427 printf("\r\n");
jjohnle 3:f282664610ba 428 printf(" Motor Temperature (Celsius) = %f",motorTemp);
jjohnle 3:f282664610ba 429 printf("\r\n");
jjohnle 3:f282664610ba 430 printf(" DC Bus (Ah) = %f",DCBusAmpHours);
jjohnle 3:f282664610ba 431 printf("\r\n");
jjohnle 3:f282664610ba 432 printf(" Odometer (Distance) (m) = %f",odometerValue);
jjohnle 3:f282664610ba 433 printf("\r\n");
jjohnle 4:7d027ceba0a0 434
jjohnle 4:7d027ceba0a0 435
jjohnle 3:f282664610ba 436 // blinking LED
jjohnle 3:f282664610ba 437 LED8 = !LED8;
jjohnle 3:f282664610ba 438 if (!can1.write(CANMessage(id, (char*)data, 8))) //send current and velocity over car CAN
jjohnle 3:f282664610ba 439 printf("Car CAN failed \n\r");
jjohnle 3:f282664610ba 440 }
jjohnle 3:f282664610ba 441 alive++;
jjohnle 3:f282664610ba 442 }
jjohnle 3:f282664610ba 443 }