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@0:c61d18b01f8c, 2019-11-09 (annotated)

Committer:
jjohnle
Date:
Sat Nov 09 15:41:47 2019 +0000
Revision:
0:c61d18b01f8c
Child:
1:863bdd011cf8
motor control

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jjohnle 0:c61d18b01f8c 1 #include "mbed.h"
jjohnle 0:c61d18b01f8c 2 #include "shared_values.h"
jjohnle 0:c61d18b01f8c 3
jjohnle 0:c61d18b01f8c 4 #define DC_BASE 0x220 // Driver controls base address
jjohnle 0:c61d18b01f8c 5 #define DC_DRIVE 0x01 // Offset for motor drive command
jjohnle 0:c61d18b01f8c 6 #define DC_POWER 0x02 // Offset for motor power command
jjohnle 0:c61d18b01f8c 7 #define DC_RESET 0x03 // Offset for reset command
jjohnle 0:c61d18b01f8c 8 #define DC_SWITCH 0x04 // Offset for phase current measurement
jjohnle 0:c61d18b01f8c 9
jjohnle 0:c61d18b01f8c 10 #define MC_BASE 0x240 // Motor controls base address
jjohnle 0:c61d18b01f8c 11 #define MC_BUS 0x02 // Bus measurement offset
jjohnle 0:c61d18b01f8c 12 #define MC_VEL 0x03 // Velocity measurement offset
jjohnle 0:c61d18b01f8c 13 #define MC_PHCUR 0x04 // Phase Current offset
jjohnle 0:c61d18b01f8c 14 #define MC_VOVEC 0x05 // Voltage Vector offset
jjohnle 0:c61d18b01f8c 15 #define MC_CUVEC 0x06 // current vector offset
jjohnle 0:c61d18b01f8c 16 #define MC_BEMF 0x07 // back emf offset
jjohnle 0:c61d18b01f8c 17 #define MC_TEMP 0x0B // heat sink and motor temp offset
jjohnle 0:c61d18b01f8c 18 #define MC_AMPH 0x0E // odometer and bus amp ohours measuremeant
jjohnle 0:c61d18b01f8c 19 #define MAX_VELOCITY 100 // motor velocity in m/s
jjohnle 0:c61d18b01f8c 20 #define MAX_CURRENT 1.0 // desired motor current as percentage of max current
jjohnle 0:c61d18b01f8c 21
jjohnle 0:c61d18b01f8c 22 #define DC_BUS_CURRENT 0x900
jjohnle 0:c61d18b01f8c 23 #define DC_BUS_VOLTAGE 0x901
jjohnle 0:c61d18b01f8c 24 #define PHASE_B_CURRENT 0x902
jjohnle 0:c61d18b01f8c 25 #define PHASE_C_CURRENT 0x903
jjohnle 0:c61d18b01f8c 26 #define VEHICLE_VELOCITY 0x904
jjohnle 0:c61d18b01f8c 27 #define MOTOR_VELOCITY 0x905
jjohnle 0:c61d18b01f8c 28 #define VD 0x906
jjohnle 0:c61d18b01f8c 29 #define VQ 0x907
jjohnle 0:c61d18b01f8c 30 #define ID 0x908
jjohnle 0:c61d18b01f8c 31 #define IQ 0x909
jjohnle 0:c61d18b01f8c 32 #define BEMFD 0x90A
jjohnle 0:c61d18b01f8c 33 #define BEMFQ 0x90B
jjohnle 0:c61d18b01f8c 34 #define HEAT_SINK_TEMPERATURE 0x90C
jjohnle 0:c61d18b01f8c 35 #define MOTOR_TEMPERATURE 0x90D
jjohnle 0:c61d18b01f8c 36 #define DC_BUS_AMP_HOURS 0x90E
jjohnle 0:c61d18b01f8c 37 #define ODOMETER 0x90F
jjohnle 0:c61d18b01f8c 38
jjohnle 0:c61d18b01f8c 39 float current = MAX_CURRENT;
jjohnle 0:c61d18b01f8c 40 float velocity = MAX_VELOCITY;
jjohnle 0:c61d18b01f8c 41 float bus_current = MAX_CURRENT;
jjohnle 0:c61d18b01f8c 42 double pedal_position;
jjohnle 0:c61d18b01f8c 43 double avgval;
jjohnle 0:c61d18b01f8c 44 float data[2];
jjohnle 0:c61d18b01f8c 45 float data2[2];
jjohnle 0:c61d18b01f8c 46 float meas = 0;
jjohnle 0:c61d18b01f8c 47 int n;
jjohnle 0:c61d18b01f8c 48 int dummy;
jjohnle 0:c61d18b01f8c 49 int alive;
jjohnle 0:c61d18b01f8c 50
jjohnle 0:c61d18b01f8c 51 int id = DC_BASE + DC_DRIVE;
jjohnle 0:c61d18b01f8c 52 int id2 = DC_BASE + DC_POWER;
jjohnle 0:c61d18b01f8c 53 int id3 = MC_BASE + DC_POWER;
jjohnle 0:c61d18b01f8c 54
jjohnle 0:c61d18b01f8c 55 CAN can1(PD_0, PD_1 /*, 125000*/); // can1 is car CAN (Rx, Tx, speed)
jjohnle 0:c61d18b01f8c 56 CAN can2(PB_5, PB_6 /*, 50000*/); // can2 is motor controller CAN (Rx, Tx, speed)
jjohnle 0:c61d18b01f8c 57 AnalogIn poop(PB_0);
jjohnle 0:c61d18b01f8c 58 Serial pc(USBTX, USBRX);
jjohnle 0:c61d18b01f8c 59
jjohnle 0:c61d18b01f8c 60 // https://stackoverflow.com/questions/24420246/c-function-to-convert-float-to-byte-array
jjohnle 0:c61d18b01f8c 61 void float2Bytes(float val,uint8_t* bytes_array){
jjohnle 0:c61d18b01f8c 62 uint8_t temp;
jjohnle 0:c61d18b01f8c 63 // Create union of shared memory space
jjohnle 0:c61d18b01f8c 64 union {
jjohnle 0:c61d18b01f8c 65 float float_variable;
jjohnle 0:c61d18b01f8c 66 uint8_t temp_array[4];
jjohnle 0:c61d18b01f8c 67 } u;
jjohnle 0:c61d18b01f8c 68 // Overite bytes of union with float variable
jjohnle 0:c61d18b01f8c 69 u.float_variable = val;
jjohnle 0:c61d18b01f8c 70 // Assign bytes to input array
jjohnle 0:c61d18b01f8c 71 memcpy(bytes_array, u.temp_array, 4);
jjohnle 0:c61d18b01f8c 72 temp = bytes_array[3];
jjohnle 0:c61d18b01f8c 73 bytes_array[3] = bytes_array[0];
jjohnle 0:c61d18b01f8c 74 bytes_array[0] = temp;
jjohnle 0:c61d18b01f8c 75 temp = bytes_array[2];
jjohnle 0:c61d18b01f8c 76 bytes_array[2] = bytes_array[1];
jjohnle 0:c61d18b01f8c 77 bytes_array[1] = temp;
jjohnle 0:c61d18b01f8c 78 }
jjohnle 0:c61d18b01f8c 79
jjohnle 0:c61d18b01f8c 80 float bytes2Float(uint8_t* bytes_array) {
jjohnle 0:c61d18b01f8c 81 union {
jjohnle 0:c61d18b01f8c 82 float f;
jjohnle 0:c61d18b01f8c 83 uint8_t b[4];
jjohnle 0:c61d18b01f8c 84 } u;
jjohnle 0:c61d18b01f8c 85 u.b[3] = bytes_array[0];
jjohnle 0:c61d18b01f8c 86 u.b[2] = bytes_array[1];
jjohnle 0:c61d18b01f8c 87 u.b[1] = bytes_array[2];
jjohnle 0:c61d18b01f8c 88 u.b[0] = bytes_array[3];
jjohnle 0:c61d18b01f8c 89 return u.f;
jjohnle 0:c61d18b01f8c 90 }
jjohnle 0:c61d18b01f8c 91
jjohnle 0:c61d18b01f8c 92 union {
jjohnle 0:c61d18b01f8c 93 char rcvdata[4];
jjohnle 0:c61d18b01f8c 94 float rxdata;
jjohnle 0:c61d18b01f8c 95 } urxdata;
jjohnle 0:c61d18b01f8c 96
jjohnle 0:c61d18b01f8c 97 CANMessage msg;
jjohnle 0:c61d18b01f8c 98 char rdata[8];
jjohnle 0:c61d18b01f8c 99
jjohnle 0:c61d18b01f8c 100
jjohnle 0:c61d18b01f8c 101 void pedal()
jjohnle 0:c61d18b01f8c 102 {
jjohnle 0:c61d18b01f8c 103 while (1)
jjohnle 0:c61d18b01f8c 104 {
jjohnle 0:c61d18b01f8c 105 n = 0;
jjohnle 0:c61d18b01f8c 106 avgval = 0.0;
jjohnle 0:c61d18b01f8c 107 while (n < 100)
jjohnle 0:c61d18b01f8c 108 {
jjohnle 0:c61d18b01f8c 109 meas = poop.read();
jjohnle 0:c61d18b01f8c 110 avgval = avgval + meas;
jjohnle 0:c61d18b01f8c 111 n++;
jjohnle 0:c61d18b01f8c 112 }
jjohnle 0:c61d18b01f8c 113 pedal_position = avgval / 100.0;
jjohnle 0:c61d18b01f8c 114
jjohnle 0:c61d18b01f8c 115 current = MAX_CURRENT * pedal_position;
jjohnle 0:c61d18b01f8c 116 velocity = 9.0;
jjohnle 0:c61d18b01f8c 117
jjohnle 0:c61d18b01f8c 118 data[1] = current; // Flipped because of endianness
jjohnle 0:c61d18b01f8c 119 data[0] = velocity;
jjohnle 0:c61d18b01f8c 120
jjohnle 0:c61d18b01f8c 121 if (!can2.write(CANMessage(id, (char *)data, 8))) // send current and velocity to Tritum
jjohnle 0:c61d18b01f8c 122 printf("Drive failed \n\r");
jjohnle 0:c61d18b01f8c 123
jjohnle 0:c61d18b01f8c 124 data2[1] = bus_current;
jjohnle 0:c61d18b01f8c 125 data2[0] = 0.0;
jjohnle 0:c61d18b01f8c 126 if (!can2.write(CANMessage(id2, (char *)data2, 8)))
jjohnle 0:c61d18b01f8c 127 dummy = 0;
jjohnle 0:c61d18b01f8c 128
jjohnle 0:c61d18b01f8c 129 wait_ms(10); // Need message every 250ms to maintain operation
jjohnle 0:c61d18b01f8c 130 }
jjohnle 0:c61d18b01f8c 131 }
jjohnle 0:c61d18b01f8c 132
jjohnle 0:c61d18b01f8c 133
jjohnle 0:c61d18b01f8c 134 void receiveCAN()
jjohnle 0:c61d18b01f8c 135 {
jjohnle 0:c61d18b01f8c 136 can1.frequency(125000);
jjohnle 0:c61d18b01f8c 137 can2.frequency(50000);
jjohnle 0:c61d18b01f8c 138 while (1)
jjohnle 0:c61d18b01f8c 139 {
jjohnle 0:c61d18b01f8c 140 pc.printf("inside thread \r\n");
jjohnle 0:c61d18b01f8c 141 if (can2.read(msg) && msg.id == (MC_BASE + MC_BUS))
jjohnle 0:c61d18b01f8c 142 {
jjohnle 0:c61d18b01f8c 143 for (int i = 0; i < msg.len; i++)
jjohnle 0:c61d18b01f8c 144 {
jjohnle 0:c61d18b01f8c 145 rdata[i] = msg.data[i];
jjohnle 0:c61d18b01f8c 146 }
jjohnle 0:c61d18b01f8c 147 urxdata.rcvdata[3] = rdata[7];
jjohnle 0:c61d18b01f8c 148 urxdata.rcvdata[2] = rdata[6];
jjohnle 0:c61d18b01f8c 149 urxdata.rcvdata[1] = rdata[5];
jjohnle 0:c61d18b01f8c 150 urxdata.rcvdata[0] = rdata[4];
jjohnle 0:c61d18b01f8c 151 DCbuscur = urxdata.rxdata;
jjohnle 0:c61d18b01f8c 152 urxdata.rcvdata[3] = rdata[3];
jjohnle 0:c61d18b01f8c 153 urxdata.rcvdata[2] = rdata[2];
jjohnle 0:c61d18b01f8c 154 urxdata.rcvdata[1] = rdata[1];
jjohnle 0:c61d18b01f8c 155 urxdata.rcvdata[0] = rdata[0];
jjohnle 0:c61d18b01f8c 156 DCbusvolt = urxdata.rxdata;
jjohnle 0:c61d18b01f8c 157 }
jjohnle 0:c61d18b01f8c 158 else if (can2.read(msg) && msg.id == (MC_BASE + MC_VEL))
jjohnle 0:c61d18b01f8c 159 {
jjohnle 0:c61d18b01f8c 160 for (int i = 0; i < msg.len; i++)
jjohnle 0:c61d18b01f8c 161 {
jjohnle 0:c61d18b01f8c 162 rdata[i] = msg.data[i];
jjohnle 0:c61d18b01f8c 163 }
jjohnle 0:c61d18b01f8c 164 urxdata.rcvdata[3] = rdata[7];
jjohnle 0:c61d18b01f8c 165 urxdata.rcvdata[2] = rdata[6];
jjohnle 0:c61d18b01f8c 166 urxdata.rcvdata[1] = rdata[5];
jjohnle 0:c61d18b01f8c 167 urxdata.rcvdata[0] = rdata[4];
jjohnle 0:c61d18b01f8c 168 vehicleVel = urxdata.rxdata;
jjohnle 0:c61d18b01f8c 169 urxdata.rcvdata[3] = rdata[3];
jjohnle 0:c61d18b01f8c 170 urxdata.rcvdata[2] = rdata[2];
jjohnle 0:c61d18b01f8c 171 urxdata.rcvdata[1] = rdata[1];
jjohnle 0:c61d18b01f8c 172 urxdata.rcvdata[0] = rdata[0];
jjohnle 0:c61d18b01f8c 173 motorVel = urxdata.rxdata;
jjohnle 0:c61d18b01f8c 174 wait_ms(10); // wait to reset
jjohnle 0:c61d18b01f8c 175 }
jjohnle 0:c61d18b01f8c 176
jjohnle 0:c61d18b01f8c 177 // reading phase currents
jjohnle 0:c61d18b01f8c 178 else if (can2.read(msg) && msg.id == (MC_BASE + MC_PHCUR))
jjohnle 0:c61d18b01f8c 179 {
jjohnle 0:c61d18b01f8c 180 for (int i = 0; i < msg.len; i++)
jjohnle 0:c61d18b01f8c 181 {
jjohnle 0:c61d18b01f8c 182 rdata[i] = msg.data[i];
jjohnle 0:c61d18b01f8c 183 }
jjohnle 0:c61d18b01f8c 184 urxdata.rcvdata[3] = rdata[7];
jjohnle 0:c61d18b01f8c 185 urxdata.rcvdata[2] = rdata[6];
jjohnle 0:c61d18b01f8c 186 urxdata.rcvdata[1] = rdata[5];
jjohnle 0:c61d18b01f8c 187 urxdata.rcvdata[0] = rdata[4];
jjohnle 0:c61d18b01f8c 188 phaseCcurrent = urxdata.rxdata;
jjohnle 0:c61d18b01f8c 189 urxdata.rcvdata[3] = rdata[3];
jjohnle 0:c61d18b01f8c 190 urxdata.rcvdata[2] = rdata[2];
jjohnle 0:c61d18b01f8c 191 urxdata.rcvdata[1] = rdata[1];
jjohnle 0:c61d18b01f8c 192 urxdata.rcvdata[0] = rdata[0];
jjohnle 0:c61d18b01f8c 193 phaseBcurrent = urxdata.rxdata;
jjohnle 0:c61d18b01f8c 194 wait_ms(10); // wait to reset
jjohnle 0:c61d18b01f8c 195 }
jjohnle 0:c61d18b01f8c 196
jjohnle 0:c61d18b01f8c 197 // reading motor voltage vector
jjohnle 0:c61d18b01f8c 198 else if (can2.read(msg) && msg.id == (MC_BASE + MC_VOVEC))
jjohnle 0:c61d18b01f8c 199 {
jjohnle 0:c61d18b01f8c 200 for (int i = 0; i < msg.len; i++)
jjohnle 0:c61d18b01f8c 201 {
jjohnle 0:c61d18b01f8c 202 rdata[i] = msg.data[i];
jjohnle 0:c61d18b01f8c 203 }
jjohnle 0:c61d18b01f8c 204 urxdata.rcvdata[3] = rdata[7];
jjohnle 0:c61d18b01f8c 205 urxdata.rcvdata[2] = rdata[6];
jjohnle 0:c61d18b01f8c 206 urxdata.rcvdata[1] = rdata[5];
jjohnle 0:c61d18b01f8c 207 urxdata.rcvdata[0] = rdata[4];
jjohnle 0:c61d18b01f8c 208 vd = urxdata.rxdata;
jjohnle 0:c61d18b01f8c 209 urxdata.rcvdata[3] = rdata[3];
jjohnle 0:c61d18b01f8c 210 urxdata.rcvdata[2] = rdata[2];
jjohnle 0:c61d18b01f8c 211 urxdata.rcvdata[1] = rdata[1];
jjohnle 0:c61d18b01f8c 212 urxdata.rcvdata[0] = rdata[0];
jjohnle 0:c61d18b01f8c 213 vq = urxdata.rxdata;
jjohnle 0:c61d18b01f8c 214 wait_ms(10); // wait to reset
jjohnle 0:c61d18b01f8c 215 }
jjohnle 0:c61d18b01f8c 216
jjohnle 0:c61d18b01f8c 217 // reading current vector
jjohnle 0:c61d18b01f8c 218 else if (can2.read(msg) && msg.id == (MC_BASE + MC_CUVEC))
jjohnle 0:c61d18b01f8c 219 {
jjohnle 0:c61d18b01f8c 220 for (int i = 0; i < msg.len; i++)
jjohnle 0:c61d18b01f8c 221 {
jjohnle 0:c61d18b01f8c 222 rdata[i] = msg.data[i];
jjohnle 0:c61d18b01f8c 223 }
jjohnle 0:c61d18b01f8c 224 urxdata.rcvdata[3] = rdata[7];
jjohnle 0:c61d18b01f8c 225 urxdata.rcvdata[2] = rdata[6];
jjohnle 0:c61d18b01f8c 226 urxdata.rcvdata[1] = rdata[5];
jjohnle 0:c61d18b01f8c 227 urxdata.rcvdata[0] = rdata[4];
jjohnle 0:c61d18b01f8c 228 Id = urxdata.rxdata;
jjohnle 0:c61d18b01f8c 229 urxdata.rcvdata[3] = rdata[3];
jjohnle 0:c61d18b01f8c 230 urxdata.rcvdata[2] = rdata[2];
jjohnle 0:c61d18b01f8c 231 urxdata.rcvdata[1] = rdata[1];
jjohnle 0:c61d18b01f8c 232 urxdata.rcvdata[0] = rdata[0];
jjohnle 0:c61d18b01f8c 233 Iq = urxdata.rxdata;
jjohnle 0:c61d18b01f8c 234 wait_ms(10); // wait to reset
jjohnle 0:c61d18b01f8c 235 }
jjohnle 0:c61d18b01f8c 236
jjohnle 0:c61d18b01f8c 237 // reading back emf
jjohnle 0:c61d18b01f8c 238 else if (can2.read(msg) && msg.id == (MC_BASE + MC_BEMF))
jjohnle 0:c61d18b01f8c 239 {
jjohnle 0:c61d18b01f8c 240 for (int i = 0; i < msg.len; i++)
jjohnle 0:c61d18b01f8c 241 {
jjohnle 0:c61d18b01f8c 242 rdata[i] = msg.data[i];
jjohnle 0:c61d18b01f8c 243 }
jjohnle 0:c61d18b01f8c 244 urxdata.rcvdata[3] = rdata[7];
jjohnle 0:c61d18b01f8c 245 urxdata.rcvdata[2] = rdata[6];
jjohnle 0:c61d18b01f8c 246 urxdata.rcvdata[1] = rdata[5];
jjohnle 0:c61d18b01f8c 247 urxdata.rcvdata[0] = rdata[4];
jjohnle 0:c61d18b01f8c 248 BEMFd = urxdata.rxdata;
jjohnle 0:c61d18b01f8c 249 urxdata.rcvdata[3] = rdata[3];
jjohnle 0:c61d18b01f8c 250 urxdata.rcvdata[2] = rdata[2];
jjohnle 0:c61d18b01f8c 251 urxdata.rcvdata[1] = rdata[1];
jjohnle 0:c61d18b01f8c 252 urxdata.rcvdata[0] = rdata[0];
jjohnle 0:c61d18b01f8c 253 BEMFq = urxdata.rxdata;
jjohnle 0:c61d18b01f8c 254 wait_ms(10); // wait to reset
jjohnle 0:c61d18b01f8c 255 }
jjohnle 0:c61d18b01f8c 256
jjohnle 0:c61d18b01f8c 257 // reading heatsink and motor temp
jjohnle 0:c61d18b01f8c 258 else if (can2.read(msg) && msg.id == (MC_BASE + MC_TEMP))
jjohnle 0:c61d18b01f8c 259 {
jjohnle 0:c61d18b01f8c 260 for (int i = 0; i < msg.len; i++)
jjohnle 0:c61d18b01f8c 261 {
jjohnle 0:c61d18b01f8c 262 rdata[i] = msg.data[i];
jjohnle 0:c61d18b01f8c 263 }
jjohnle 0:c61d18b01f8c 264 urxdata.rcvdata[3] = rdata[7];
jjohnle 0:c61d18b01f8c 265 urxdata.rcvdata[2] = rdata[6];
jjohnle 0:c61d18b01f8c 266 urxdata.rcvdata[1] = rdata[5];
jjohnle 0:c61d18b01f8c 267 urxdata.rcvdata[0] = rdata[4];
jjohnle 0:c61d18b01f8c 268 heatSinkTemp = urxdata.rxdata;
jjohnle 0:c61d18b01f8c 269 urxdata.rcvdata[3] = rdata[3];
jjohnle 0:c61d18b01f8c 270 urxdata.rcvdata[2] = rdata[2];
jjohnle 0:c61d18b01f8c 271 urxdata.rcvdata[1] = rdata[1];
jjohnle 0:c61d18b01f8c 272 urxdata.rcvdata[0] = rdata[0];
jjohnle 0:c61d18b01f8c 273 motorTemp = urxdata.rxdata;
jjohnle 0:c61d18b01f8c 274 wait_ms(10); // wait to reset
jjohnle 0:c61d18b01f8c 275 }
jjohnle 0:c61d18b01f8c 276
jjohnle 0:c61d18b01f8c 277 // reading odometer and bus amp ohours measuremeant
jjohnle 0:c61d18b01f8c 278 else if (can2.read(msg) && msg.id == (MC_BASE + MC_AMPH))
jjohnle 0:c61d18b01f8c 279 {
jjohnle 0:c61d18b01f8c 280 for (int i = 0; i < msg.len; i++)
jjohnle 0:c61d18b01f8c 281 {
jjohnle 0:c61d18b01f8c 282 rdata[i] = msg.data[i];
jjohnle 0:c61d18b01f8c 283 }
jjohnle 0:c61d18b01f8c 284 urxdata.rcvdata[3] = rdata[7];
jjohnle 0:c61d18b01f8c 285 urxdata.rcvdata[2] = rdata[6];
jjohnle 0:c61d18b01f8c 286 urxdata.rcvdata[1] = rdata[5];
jjohnle 0:c61d18b01f8c 287 urxdata.rcvdata[0] = rdata[4];
jjohnle 0:c61d18b01f8c 288 DCBusAmpHours = urxdata.rxdata;
jjohnle 0:c61d18b01f8c 289 urxdata.rcvdata[3] = rdata[3];
jjohnle 0:c61d18b01f8c 290 urxdata.rcvdata[2] = rdata[2];
jjohnle 0:c61d18b01f8c 291 urxdata.rcvdata[1] = rdata[1];
jjohnle 0:c61d18b01f8c 292 urxdata.rcvdata[0] = rdata[0];
jjohnle 0:c61d18b01f8c 293 odometerValue = urxdata.rxdata;
jjohnle 0:c61d18b01f8c 294 wait_ms(10); // wait to reset
jjohnle 0:c61d18b01f8c 295 }
jjohnle 0:c61d18b01f8c 296
jjohnle 0:c61d18b01f8c 297 if (alive % 100 == 0)
jjohnle 0:c61d18b01f8c 298 {
jjohnle 0:c61d18b01f8c 299
jjohnle 0:c61d18b01f8c 300 printf("Motor board is running");
jjohnle 0:c61d18b01f8c 301 printf("\r\n");
jjohnle 0:c61d18b01f8c 302 //printf(" Requested Motor Current: %f\n\r", current);
jjohnle 0:c61d18b01f8c 303 //printf(" Requested Motor Velocity: %f\n\r", velocity);
jjohnle 0:c61d18b01f8c 304 printf(" DC Bus Current (A) = %f", DCbuscur);
jjohnle 0:c61d18b01f8c 305 printf("\r\n");
jjohnle 0:c61d18b01f8c 306 printf(" DC Bus Voltage (V) = %f", DCbusvolt);
jjohnle 0:c61d18b01f8c 307 printf("\r\n");
jjohnle 0:c61d18b01f8c 308
jjohnle 0:c61d18b01f8c 309 // Printing other values
jjohnle 0:c61d18b01f8c 310 printf(" Vehicle Velocity (RPM) = %f", vehicleVel);
jjohnle 0:c61d18b01f8c 311 printf("\r\n");
jjohnle 0:c61d18b01f8c 312 printf(" Motor Velocity (V) = %f", motorVel);
jjohnle 0:c61d18b01f8c 313 printf("\r\n");
jjohnle 0:c61d18b01f8c 314 printf(" Phase B Current (A-rms) = %f", phaseBcurrent);
jjohnle 0:c61d18b01f8c 315 printf("\r\n");
jjohnle 0:c61d18b01f8c 316 printf(" Phase C Current (A-rms) = %f", phaseCcurrent);
jjohnle 0:c61d18b01f8c 317 printf("\r\n");
jjohnle 0:c61d18b01f8c 318 printf(" Vd (V) = %f", vd);
jjohnle 0:c61d18b01f8c 319 printf("\r\n");
jjohnle 0:c61d18b01f8c 320 printf(" Vq (V) = %f", vq);
jjohnle 0:c61d18b01f8c 321 printf("\r\n");
jjohnle 0:c61d18b01f8c 322
jjohnle 0:c61d18b01f8c 323 printf(" Id (A) = %f", Id);
jjohnle 0:c61d18b01f8c 324 printf("\r\n");
jjohnle 0:c61d18b01f8c 325 printf(" Iq (A) = %f", Iq);
jjohnle 0:c61d18b01f8c 326 printf("\r\n");
jjohnle 0:c61d18b01f8c 327 printf(" BEMFd (V) = %f", BEMFd);
jjohnle 0:c61d18b01f8c 328 printf("\r\n");
jjohnle 0:c61d18b01f8c 329 printf(" BEMFq (V) = %f", BEMFq);
jjohnle 0:c61d18b01f8c 330 printf("\r\n");
jjohnle 0:c61d18b01f8c 331 printf(" Heat Sink Temperature (Celsius) = %f", heatSinkTemp);
jjohnle 0:c61d18b01f8c 332 printf("\r\n");
jjohnle 0:c61d18b01f8c 333 printf(" Motor Temperature (Celsius) = %f", motorTemp);
jjohnle 0:c61d18b01f8c 334 printf("\r\n");
jjohnle 0:c61d18b01f8c 335 printf(" DC Bus (Ah) = %f", DCBusAmpHours);
jjohnle 0:c61d18b01f8c 336 printf("\r\n");
jjohnle 0:c61d18b01f8c 337 printf(" Odometer (Distance) (m) = %f", odometerValue);
jjohnle 0:c61d18b01f8c 338 printf("\r\n");
jjohnle 0:c61d18b01f8c 339 }
jjohnle 0:c61d18b01f8c 340 }
jjohnle 0:c61d18b01f8c 341 }
jjohnle 0:c61d18b01f8c 342
jjohnle 0:c61d18b01f8c 343 int counter = 0;
jjohnle 0:c61d18b01f8c 344 int CAN_FLAG = 0;
jjohnle 0:c61d18b01f8c 345
jjohnle 0:c61d18b01f8c 346 void sendCAN()
jjohnle 0:c61d18b01f8c 347 {
jjohnle 0:c61d18b01f8c 348 while (1)
jjohnle 0:c61d18b01f8c 349 {
jjohnle 0:c61d18b01f8c 350 uint8_t bytes1[4];
jjohnle 0:c61d18b01f8c 351 float2Bytes(DCbuscur, &bytes1[0]);
jjohnle 0:c61d18b01f8c 352 if (can1.write(CANMessage(DC_BUS_CURRENT, (char*)(bytes1), 4))) {
jjohnle 0:c61d18b01f8c 353 pc.printf("Sent DC Bus Current");
jjohnle 0:c61d18b01f8c 354 }
jjohnle 0:c61d18b01f8c 355 else {
jjohnle 0:c61d18b01f8c 356 pc.printf("Cannot write to CAN\n");
jjohnle 0:c61d18b01f8c 357 }
jjohnle 0:c61d18b01f8c 358
jjohnle 0:c61d18b01f8c 359 uint8_t bytes2[4];
jjohnle 0:c61d18b01f8c 360 float2Bytes(DCbusvolt, &bytes2[0]);
jjohnle 0:c61d18b01f8c 361 if (can1.write(CANMessage(DC_BUS_VOLTAGE, (char*)(bytes2), 4))) {
jjohnle 0:c61d18b01f8c 362 pc.printf("Sent DC Bus Voltage");
jjohnle 0:c61d18b01f8c 363 }
jjohnle 0:c61d18b01f8c 364 else {
jjohnle 0:c61d18b01f8c 365 pc.printf("Cannot write to CAN\n");
jjohnle 0:c61d18b01f8c 366 }
jjohnle 0:c61d18b01f8c 367
jjohnle 0:c61d18b01f8c 368 uint8_t bytes3[4];
jjohnle 0:c61d18b01f8c 369 float2Bytes(vehicleVel, &bytes3[0]);
jjohnle 0:c61d18b01f8c 370 if (can1.write(CANMessage(VEHICLE_VELOCITY, (char*)(bytes3), 4))) {
jjohnle 0:c61d18b01f8c 371 pc.printf("Sent Vehicle Velocity (RPM)");
jjohnle 0:c61d18b01f8c 372 }
jjohnle 0:c61d18b01f8c 373 else {
jjohnle 0:c61d18b01f8c 374 pc.printf("Cannot write to CAN\n");
jjohnle 0:c61d18b01f8c 375 }
jjohnle 0:c61d18b01f8c 376
jjohnle 0:c61d18b01f8c 377 uint8_t bytes4[4];
jjohnle 0:c61d18b01f8c 378 float2Bytes(motorVel, &bytes4[0]);
jjohnle 0:c61d18b01f8c 379 if (can1.write(CANMessage(MOTOR_VELOCITY, (char*)(bytes4), 4))) {
jjohnle 0:c61d18b01f8c 380 pc.printf("Sent Motor Velocity (V)");
jjohnle 0:c61d18b01f8c 381 }
jjohnle 0:c61d18b01f8c 382 else {
jjohnle 0:c61d18b01f8c 383 pc.printf("Cannot write to CAN\n");
jjohnle 0:c61d18b01f8c 384 }
jjohnle 0:c61d18b01f8c 385
jjohnle 0:c61d18b01f8c 386 uint8_t bytes5[4];
jjohnle 0:c61d18b01f8c 387 float2Bytes(phaseBcurrent, &bytes5[0]);
jjohnle 0:c61d18b01f8c 388 if (can1.write(CANMessage(PHASE_B_CURRENT, (char*)(bytes5), 4))) {
jjohnle 0:c61d18b01f8c 389 pc.printf("Sent Phase B Current");
jjohnle 0:c61d18b01f8c 390 }
jjohnle 0:c61d18b01f8c 391 else {
jjohnle 0:c61d18b01f8c 392 pc.printf("Cannot write to CAN\n");
jjohnle 0:c61d18b01f8c 393 }
jjohnle 0:c61d18b01f8c 394
jjohnle 0:c61d18b01f8c 395 uint8_t bytes6[4];
jjohnle 0:c61d18b01f8c 396 float2Bytes(phaseCcurrent, &bytes6[0]);
jjohnle 0:c61d18b01f8c 397 if (can1.write(CANMessage(PHASE_C_CURRENT, (char*)(bytes6), 4))) {
jjohnle 0:c61d18b01f8c 398 pc.printf("Sent Phase C Current");
jjohnle 0:c61d18b01f8c 399 }
jjohnle 0:c61d18b01f8c 400 else {
jjohnle 0:c61d18b01f8c 401 pc.printf("Cannot write to CAN\n");
jjohnle 0:c61d18b01f8c 402 }
jjohnle 0:c61d18b01f8c 403
jjohnle 0:c61d18b01f8c 404 uint8_t bytes7[4];
jjohnle 0:c61d18b01f8c 405 float2Bytes(vd, &bytes7[0]);
jjohnle 0:c61d18b01f8c 406 if (can1.write(CANMessage(VD, (char*)(bytes7), 4))) {
jjohnle 0:c61d18b01f8c 407 pc.printf("Sent Vd (V)");
jjohnle 0:c61d18b01f8c 408 }
jjohnle 0:c61d18b01f8c 409 else {
jjohnle 0:c61d18b01f8c 410 pc.printf("Cannot write to CAN\n");
jjohnle 0:c61d18b01f8c 411 }
jjohnle 0:c61d18b01f8c 412
jjohnle 0:c61d18b01f8c 413 uint8_t bytes8[4];
jjohnle 0:c61d18b01f8c 414 float2Bytes(vq, &bytes8[0]);
jjohnle 0:c61d18b01f8c 415 if (can1.write(CANMessage(VQ, (char*)(bytes8), 4))) {
jjohnle 0:c61d18b01f8c 416 pc.printf("Sent Vq (V)");
jjohnle 0:c61d18b01f8c 417 }
jjohnle 0:c61d18b01f8c 418 else {
jjohnle 0:c61d18b01f8c 419 pc.printf("Cannot write to CAN\n");
jjohnle 0:c61d18b01f8c 420 }
jjohnle 0:c61d18b01f8c 421
jjohnle 0:c61d18b01f8c 422 uint8_t bytes9[4];
jjohnle 0:c61d18b01f8c 423 float2Bytes(Id, &bytes9[0]);
jjohnle 0:c61d18b01f8c 424 if (can1.write(CANMessage(ID, (char*)(bytes9), 4))) {
jjohnle 0:c61d18b01f8c 425 pc.printf("Sent Id (A)");
jjohnle 0:c61d18b01f8c 426 }
jjohnle 0:c61d18b01f8c 427 else {
jjohnle 0:c61d18b01f8c 428 pc.printf("Cannot write to CAN\n");
jjohnle 0:c61d18b01f8c 429 }
jjohnle 0:c61d18b01f8c 430
jjohnle 0:c61d18b01f8c 431 uint8_t bytes10[4];
jjohnle 0:c61d18b01f8c 432 float2Bytes(Iq, &bytes10[0]);
jjohnle 0:c61d18b01f8c 433 if (can1.write(CANMessage(IQ, (char*)(bytes10), 4))) {
jjohnle 0:c61d18b01f8c 434 pc.printf("Sent Iq (A)");
jjohnle 0:c61d18b01f8c 435 }
jjohnle 0:c61d18b01f8c 436 else {
jjohnle 0:c61d18b01f8c 437 pc.printf("Cannot write to CAN\n");
jjohnle 0:c61d18b01f8c 438 }
jjohnle 0:c61d18b01f8c 439
jjohnle 0:c61d18b01f8c 440 uint8_t bytes11[4];
jjohnle 0:c61d18b01f8c 441 float2Bytes(BEMFd, &bytes11[0]);
jjohnle 0:c61d18b01f8c 442 if (can1.write(CANMessage(BEMFD, (char*)(bytes11), 4))) {
jjohnle 0:c61d18b01f8c 443 pc.printf("Sent BEMFd");
jjohnle 0:c61d18b01f8c 444 }
jjohnle 0:c61d18b01f8c 445 else {
jjohnle 0:c61d18b01f8c 446 pc.printf("Cannot write to CAN\n");
jjohnle 0:c61d18b01f8c 447 }
jjohnle 0:c61d18b01f8c 448
jjohnle 0:c61d18b01f8c 449 uint8_t bytes12[4];
jjohnle 0:c61d18b01f8c 450 float2Bytes(BEMFq, &bytes12[0]);
jjohnle 0:c61d18b01f8c 451 if (can1.write(CANMessage(BEMFQ, (char*)(bytes12), 4))) {
jjohnle 0:c61d18b01f8c 452 pc.printf("Sent BEMFq");
jjohnle 0:c61d18b01f8c 453 }
jjohnle 0:c61d18b01f8c 454 else {
jjohnle 0:c61d18b01f8c 455 pc.printf("Cannot write to CAN\n");
jjohnle 0:c61d18b01f8c 456 }
jjohnle 0:c61d18b01f8c 457
jjohnle 0:c61d18b01f8c 458 uint8_t bytes13[4];
jjohnle 0:c61d18b01f8c 459 float2Bytes(heatSinkTemp, &bytes13[0]);
jjohnle 0:c61d18b01f8c 460 if (can1.write(CANMessage(HEAT_SINK_TEMPERATURE, (char*)(bytes13), 4))) {
jjohnle 0:c61d18b01f8c 461 pc.printf("Sent Heat Sink Temperature (Celsius)");
jjohnle 0:c61d18b01f8c 462 }
jjohnle 0:c61d18b01f8c 463 else {
jjohnle 0:c61d18b01f8c 464 pc.printf("Cannot write to CAN\n");
jjohnle 0:c61d18b01f8c 465 }
jjohnle 0:c61d18b01f8c 466
jjohnle 0:c61d18b01f8c 467 uint8_t bytes14[4];
jjohnle 0:c61d18b01f8c 468 float2Bytes(motorTemp, &bytes14[0]);
jjohnle 0:c61d18b01f8c 469 if (can1.write(CANMessage(MOTOR_TEMPERATURE, (char*)(bytes14), 4))) {
jjohnle 0:c61d18b01f8c 470 pc.printf("Sent Motor Temperature (Celsius)");
jjohnle 0:c61d18b01f8c 471 }
jjohnle 0:c61d18b01f8c 472 else {
jjohnle 0:c61d18b01f8c 473 pc.printf("Cannot write to CAN\n");
jjohnle 0:c61d18b01f8c 474 }
jjohnle 0:c61d18b01f8c 475
jjohnle 0:c61d18b01f8c 476 uint8_t bytes15[4];
jjohnle 0:c61d18b01f8c 477 float2Bytes(DCBusAmpHours, &bytes15[0]);
jjohnle 0:c61d18b01f8c 478 if (can1.write(CANMessage(DC_BUS_AMP_HOURS, (char*)(bytes15), 4))) {
jjohnle 0:c61d18b01f8c 479 pc.printf("Sent DC Bus (Ah)");
jjohnle 0:c61d18b01f8c 480 }
jjohnle 0:c61d18b01f8c 481 else {
jjohnle 0:c61d18b01f8c 482 pc.printf("Cannot write to CAN\n");
jjohnle 0:c61d18b01f8c 483 }
jjohnle 0:c61d18b01f8c 484
jjohnle 0:c61d18b01f8c 485 uint8_t bytes16[4];
jjohnle 0:c61d18b01f8c 486 float2Bytes(odometerValue, &bytes16[0]);
jjohnle 0:c61d18b01f8c 487 if (can1.write(CANMessage(ODOMETER, (char*)(bytes16), 4))) {
jjohnle 0:c61d18b01f8c 488 pc.printf("Sent Odometer (Distance) (m)");
jjohnle 0:c61d18b01f8c 489 }
jjohnle 0:c61d18b01f8c 490 else {
jjohnle 0:c61d18b01f8c 491 pc.printf("Cannot write to CAN\n");
jjohnle 0:c61d18b01f8c 492 }
jjohnle 0:c61d18b01f8c 493 }
jjohnle 0:c61d18b01f8c 494 }
jjohnle 0:c61d18b01f8c 495
jjohnle 0:c61d18b01f8c 496
jjohnle 0:c61d18b01f8c 497 int main()
jjohnle 0:c61d18b01f8c 498 {
jjohnle 0:c61d18b01f8c 499 Thread recc(receiveCAN);
jjohnle 0:c61d18b01f8c 500 wait(1);
jjohnle 0:c61d18b01f8c 501 Thread Indicators(pedal);
jjohnle 0:c61d18b01f8c 502 wait(2);
jjohnle 0:c61d18b01f8c 503 // Thread send(sendCAN);
jjohnle 0:c61d18b01f8c 504
jjohnle 0:c61d18b01f8c 505 while (1)
jjohnle 0:c61d18b01f8c 506 {
jjohnle 0:c61d18b01f8c 507 uint8_t bytes1[4];
jjohnle 0:c61d18b01f8c 508 float foo = 42.0;
jjohnle 0:c61d18b01f8c 509 float2Bytes(foo, &bytes1[0]);
jjohnle 0:c61d18b01f8c 510 if (can1.write(CANMessage(DC_BUS_CURRENT, (char*)(bytes1), 4))) {
jjohnle 0:c61d18b01f8c 511 pc.printf("Sent DC Bus Current = %f", foo);
jjohnle 0:c61d18b01f8c 512 }
jjohnle 0:c61d18b01f8c 513 else {
jjohnle 0:c61d18b01f8c 514 pc.printf("Cannot write to CAN\n");
jjohnle 0:c61d18b01f8c 515 }
jjohnle 0:c61d18b01f8c 516 wait(0.1);
jjohnle 0:c61d18b01f8c 517 }
jjohnle 0:c61d18b01f8c 518 }