Gabriel Bernatchez / Mbed 2 deprecated EV

test

Dependencies:   SDFileSystem mbed

main.cpp@1:ae56cde0a4d4, 2017-02-28 (annotated)

Committer:
ediukill
Date:
Tue Feb 28 20:44:40 2017 +0000
Revision:
1:ae56cde0a4d4
Parent:
0:bc9f2c007928 
asd

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ediukill 0:bc9f2c007928 1 #include "mbed.h"
ediukill 0:bc9f2c007928 2
ediukill 0:bc9f2c007928 3 #include "SDFileSystem.h"
ediukill 0:bc9f2c007928 4
ediukill 0:bc9f2c007928 5 //Create an SDFileSystem object
ediukill 0:bc9f2c007928 6 SDFileSystem sd(PE_6, PE_5, PE_2, PE_4, "sd");
ediukill 0:bc9f2c007928 7 void pm100dx_init();
ediukill 0:bc9f2c007928 8
ediukill 0:bc9f2c007928 9 void pm100dx_can_tx(uint32_t id, char data[]);
ediukill 0:bc9f2c007928 10 void pm100dx_can_read(uint32_t id, char data[]);
ediukill 0:bc9f2c007928 11 float pm100dx_getMaxControllerT();
ediukill 0:bc9f2c007928 12 void pm100dx_set_torque(float torque, int dir, int enable, int discharge);
ediukill 0:bc9f2c007928 13 uint8_t pm100dx_set_voltage_limits(float dc_overvoltage, float dc_undervoltage);
ediukill 0:bc9f2c007928 14 uint8_t pm100dx_set_temperature_limits(float inverter_over_temp,
ediukill 0:bc9f2c007928 15 float motor_over_temp);
ediukill 0:bc9f2c007928 16 uint8_t pm100dx_set_current_limits(float iq_limit, float id_limit);
ediukill 0:bc9f2c007928 17 uint8_t pm100dx_set_torque_config(float motor_limit, float regen_limit,
ediukill 0:bc9f2c007928 18 float brake_limit, float zero_torque_temp, float full_torque_temp);
ediukill 0:bc9f2c007928 19 uint8_t pm100dx_set_torque_pid(float kp_torque, float ki_torque,
ediukill 0:bc9f2c007928 20 float kd_torque, float kip_torque, float torque_rate_limit);
ediukill 0:bc9f2c007928 21
ediukill 0:bc9f2c007928 22 void pm100dx_command(int address, int p_data[], int rw);
ediukill 0:bc9f2c007928 23 void pm100dx_clear_fault();
ediukill 0:bc9f2c007928 24
ediukill 0:bc9f2c007928 25 float pm100dx_read_temperature(char data[]);
ediukill 0:bc9f2c007928 26 float pm100dx_read_LV(char data[]);
ediukill 0:bc9f2c007928 27 float pm100dx_read_Torque(char data[]);
ediukill 0:bc9f2c007928 28 float pm100dx_read_HV(char data[]);
ediukill 0:bc9f2c007928 29 float pm100dx_read_Current(char data[]);
ediukill 0:bc9f2c007928 30 float pm100dx_read_AngVel(char data[]);
ediukill 0:bc9f2c007928 31 float pm100dx_read_Power(char data[]);
ediukill 0:bc9f2c007928 32
ediukill 0:bc9f2c007928 33
ediukill 0:bc9f2c007928 34 #include "stdint.h"
ediukill 0:bc9f2c007928 35 Ticker ticker;
ediukill 0:bc9f2c007928 36 DigitalOut led1(LED1);
ediukill 0:bc9f2c007928 37 DigitalOut led2(LED2);
ediukill 0:bc9f2c007928 38 CAN can1(PD_0 , PD_1 );
ediukill 0:bc9f2c007928 39
ediukill 0:bc9f2c007928 40 volatile float pm100dx_temp_modA;
ediukill 0:bc9f2c007928 41 volatile float pm100dx_temp_modB;
ediukill 0:bc9f2c007928 42 volatile float pm100dx_temp_modC;
ediukill 0:bc9f2c007928 43 volatile float pm100dx_temp_gate;
ediukill 0:bc9f2c007928 44 volatile float pm100dx_temp_motor;
ediukill 0:bc9f2c007928 45 volatile float pm100dx_mot_speed;
ediukill 0:bc9f2c007928 46 volatile float pm100dx_current_phaseA;
ediukill 0:bc9f2c007928 47 volatile float pm100dx_current_phaseB;
ediukill 0:bc9f2c007928 48 volatile float pm100dx_current_phaseC;
ediukill 0:bc9f2c007928 49 volatile float pm100dx_current_phaseDC;
ediukill 0:bc9f2c007928 50 volatile float pm100dx_voltage_DCbus;
ediukill 0:bc9f2c007928 51 volatile float pm100dx_voltage_Output;
ediukill 0:bc9f2c007928 52 volatile float pm100dx_voltage_phaseAB;
ediukill 0:bc9f2c007928 53 volatile float pm100dx_voltage_phaseBC;
ediukill 0:bc9f2c007928 54 volatile float pm100dx_voltage_1_5V;
ediukill 0:bc9f2c007928 55 volatile float pm100dx_voltage_2_5V;
ediukill 0:bc9f2c007928 56 volatile float pm100dx_voltage_5V;
ediukill 0:bc9f2c007928 57 volatile float pm100dx_voltage_12V;
ediukill 0:bc9f2c007928 58 volatile int pm100dx_inv_state;
ediukill 0:bc9f2c007928 59 volatile int pm100dx_inv_run_state = 0;
ediukill 0:bc9f2c007928 60 volatile int pm100dx_inv_command_mode = 0;
ediukill 0:bc9f2c007928 61 volatile int pm100dx_inv_enable_state = 0;
ediukill 0:bc9f2c007928 62 volatile int pm100dx_inv_enable_lockout = 0;
ediukill 0:bc9f2c007928 63 volatile int pm100dx_inv_dir_command = 0;
ediukill 0:bc9f2c007928 64 volatile uint32_t pm100dx_fault[2] = { 0, 0 };
ediukill 0:bc9f2c007928 65
ediukill 0:bc9f2c007928 66 volatile float pm100dx_commanded_torque;
ediukill 0:bc9f2c007928 67 volatile float pm100dx_feedback_torque;
ediukill 0:bc9f2c007928 68 volatile int pm100dx_date; //stored as ddmmyyyy ( first bit is day, second is month, third and fourth is year)
ediukill 0:bc9f2c007928 69
ediukill 0:bc9f2c007928 70
ediukill 0:bc9f2c007928 71 #define BMS_ID 0x99
ediukill 0:bc9f2c007928 72 #define PM100DX_CAN_BASE_ID 0x100
ediukill 0:bc9f2c007928 73 char counter = 0;
ediukill 0:bc9f2c007928 74 void com_loop_vc();
ediukill 0:bc9f2c007928 75
ediukill 0:bc9f2c007928 76 void bmsRead(uint32_t id, char data[]);
ediukill 0:bc9f2c007928 77 void pm100dx_can_read(uint32_t id, char data[]);
ediukill 0:bc9f2c007928 78
ediukill 0:bc9f2c007928 79 void com_loop_pwtr(uint32_t id, char data[]);
ediukill 0:bc9f2c007928 80 void send() {
ediukill 0:bc9f2c007928 81 if(can1.write(CANMessage(1337, &counter, 1))) {
ediukill 0:bc9f2c007928 82 printf("Message sent: %d\n", counter);
ediukill 0:bc9f2c007928 83 counter++;
ediukill 0:bc9f2c007928 84 }
ediukill 0:bc9f2c007928 85 led1 = !led1;
ediukill 0:bc9f2c007928 86 }
ediukill 0:bc9f2c007928 87
ediukill 0:bc9f2c007928 88 int main() {
ediukill 0:bc9f2c007928 89 ticker.attach(&send, 1);
ediukill 0:bc9f2c007928 90 CANMessage msg;
ediukill 0:bc9f2c007928 91 while(1) {
ediukill 0:bc9f2c007928 92 if(can1.read(msg)) {
ediukill 0:bc9f2c007928 93 char data[8];
ediukill 0:bc9f2c007928 94 for (int i=0;i<8;i++)
ediukill 0:bc9f2c007928 95 data[i]=msg.data[i];
ediukill 0:bc9f2c007928 96 com_loop_pwtr(msg.id,data);
ediukill 0:bc9f2c007928 97 printf("Message received: %d\n\n", msg.data[0]);
ediukill 0:bc9f2c007928 98 led2 = !led2;
ediukill 0:bc9f2c007928 99 }
ediukill 0:bc9f2c007928 100 pm100dx_set_torque(10, 1, 1, 0);
ediukill 0:bc9f2c007928 101 wait(0.2);
ediukill 0:bc9f2c007928 102 }
ediukill 0:bc9f2c007928 103 }
ediukill 0:bc9f2c007928 104
ediukill 0:bc9f2c007928 105
ediukill 0:bc9f2c007928 106 void com_loop_pwtr(uint32_t id, char data[])
ediukill 0:bc9f2c007928 107 {
ediukill 0:bc9f2c007928 108 switch (id) {
ediukill 0:bc9f2c007928 109 case BMS_ID ... BMS_ID + 0x1F:
ediukill 0:bc9f2c007928 110 bmsRead(id, data);
ediukill 0:bc9f2c007928 111 break;
ediukill 0:bc9f2c007928 112 case PM100DX_CAN_BASE_ID ... PM100DX_CAN_BASE_ID + 22:
ediukill 0:bc9f2c007928 113 pm100dx_can_read(id, data);
ediukill 0:bc9f2c007928 114 break;
ediukill 0:bc9f2c007928 115 }
ediukill 0:bc9f2c007928 116 }
ediukill 0:bc9f2c007928 117
ediukill 0:bc9f2c007928 118 void bmsRead(uint32_t id, char data[]){
ediukill 0:bc9f2c007928 119
ediukill 0:bc9f2c007928 120 }
ediukill 0:bc9f2c007928 121
ediukill 0:bc9f2c007928 122
ediukill 0:bc9f2c007928 123 void pm100dx_can_read(uint32_t id, char data[]){
ediukill 0:bc9f2c007928 124 int temp = (int) (id - PM100DX_CAN_BASE_ID);
ediukill 0:bc9f2c007928 125 int year;
ediukill 0:bc9f2c007928 126 int month;
ediukill 0:bc9f2c007928 127 switch (temp) { //see page 13
ediukill 0:bc9f2c007928 128 case 0: //Temp 1, 10hz
ediukill 0:bc9f2c007928 129 pm100dx_temp_modA = pm100dx_read_temperature(&data[0]); //Module A temp
ediukill 0:bc9f2c007928 130 pm100dx_temp_modB = pm100dx_read_temperature(&data[2]); //Module B temp
ediukill 0:bc9f2c007928 131 pm100dx_temp_modC = pm100dx_read_temperature(&data[4]); //Module C temp
ediukill 0:bc9f2c007928 132 pm100dx_temp_gate = pm100dx_read_temperature(&data[6]); //Gate Driver Temp
ediukill 0:bc9f2c007928 133 break;
ediukill 0:bc9f2c007928 134 case 1: // temp 2, 10hz ,disabled
ediukill 0:bc9f2c007928 135 break;
ediukill 0:bc9f2c007928 136 case 2: //temp 3, 10hz
ediukill 0:bc9f2c007928 137 pm100dx_temp_motor = pm100dx_read_temperature(&data[4]); //Motor Temp
ediukill 0:bc9f2c007928 138 break;
ediukill 0:bc9f2c007928 139 case 3: //Analog Input Voltage, 100hz, disabled
ediukill 0:bc9f2c007928 140 break;
ediukill 0:bc9f2c007928 141 case 4: //digital input status ,100hz, disabled
ediukill 0:bc9f2c007928 142 break;
ediukill 0:bc9f2c007928 143 case 5: //motor position info, 100hz
ediukill 0:bc9f2c007928 144 pm100dx_mot_speed = pm100dx_read_AngVel(&data[2]);
ediukill 0:bc9f2c007928 145 break;
ediukill 0:bc9f2c007928 146 case 6: //current info, 100hz
ediukill 0:bc9f2c007928 147 pm100dx_current_phaseA = pm100dx_read_Current(&data[0]); //PHASE A
ediukill 0:bc9f2c007928 148 pm100dx_current_phaseB = pm100dx_read_Current(&data[2]);
ediukill 0:bc9f2c007928 149 pm100dx_current_phaseC = pm100dx_read_Current(&data[4]);
ediukill 0:bc9f2c007928 150 pm100dx_current_phaseDC = pm100dx_read_Current(&data[6]);
ediukill 0:bc9f2c007928 151 break;
ediukill 0:bc9f2c007928 152 case 7: //voltage info, 100hz
ediukill 0:bc9f2c007928 153 pm100dx_voltage_DCbus = pm100dx_read_HV(&data[0]);
ediukill 0:bc9f2c007928 154 pm100dx_voltage_Output = pm100dx_read_HV(&data[2]);
ediukill 0:bc9f2c007928 155 pm100dx_voltage_phaseAB = pm100dx_read_HV(&data[4]);
ediukill 0:bc9f2c007928 156 pm100dx_voltage_phaseBC = pm100dx_read_HV(&data[6]);
ediukill 0:bc9f2c007928 157 break;
ediukill 0:bc9f2c007928 158 case 8: //flux info, 100hz , disabled
ediukill 0:bc9f2c007928 159 break;
ediukill 0:bc9f2c007928 160 case 9: // internal voltage, 10hz
ediukill 0:bc9f2c007928 161 pm100dx_voltage_1_5V = pm100dx_read_LV(&data[0]);
ediukill 0:bc9f2c007928 162 pm100dx_voltage_2_5V = pm100dx_read_LV(&data[2]);
ediukill 0:bc9f2c007928 163 pm100dx_voltage_5V = pm100dx_read_LV(&data[4]);
ediukill 0:bc9f2c007928 164 pm100dx_voltage_12V = pm100dx_read_LV(&data[6]);
ediukill 0:bc9f2c007928 165 break;
ediukill 0:bc9f2c007928 166 case 10: //internal states, 10 hz
ediukill 0:bc9f2c007928 167 pm100dx_inv_state = (int) data[2]; //Inverter state
ediukill 0:bc9f2c007928 168 pm100dx_inv_run_state = (int) data[4] & (1 << 0); //Inverter Run Mode
ediukill 0:bc9f2c007928 169 pm100dx_inv_command_mode = data[5]; //Inverter Command Mode
ediukill 0:bc9f2c007928 170 pm100dx_inv_enable_state = data[6] & (1 << 0); //Inverter Enabled
ediukill 0:bc9f2c007928 171 pm100dx_inv_enable_lockout = (int) ((data[6] & (1 << 7)) >> 7); //Inverter Enable Lockout
ediukill 0:bc9f2c007928 172 pm100dx_inv_dir_command = data[7]; //Direction Command
ediukill 0:bc9f2c007928 173 break;
ediukill 0:bc9f2c007928 174 case 11: //fault codes, 10 hz
ediukill 0:bc9f2c007928 175 pm100dx_fault[0] = (uint32_t) data[0] | (uint32_t) (data[1] << 8)
ediukill 0:bc9f2c007928 176 | (uint32_t) (data[2] << 16) | (uint32_t) (data[3] << 24);
ediukill 0:bc9f2c007928 177 pm100dx_fault[1] = (uint32_t) (data[4]) | (uint32_t) (data[5] << 8)
ediukill 0:bc9f2c007928 178 | (uint32_t) (data[6] << 16) | (uint32_t) (data[7] << 24);
ediukill 0:bc9f2c007928 179 break;
ediukill 0:bc9f2c007928 180 case 12: // torque & timer info, 100hz
ediukill 0:bc9f2c007928 181 pm100dx_commanded_torque = pm100dx_read_Torque(&data[0]); // commanded torque
ediukill 0:bc9f2c007928 182 pm100dx_feedback_torque = pm100dx_read_Torque(&data[2]); // torque feedback
ediukill 0:bc9f2c007928 183 break;
ediukill 0:bc9f2c007928 184 case 13: //modulation index, 100hz , not used
ediukill 0:bc9f2c007928 185 break;
ediukill 0:bc9f2c007928 186 case 14: // firmware info, 10hz
ediukill 0:bc9f2c007928 187 month = (int) data[4] | (int) (data[5] << 8);
ediukill 0:bc9f2c007928 188 year = ((int) (data[6]) | (int) (data[7] << 8));
ediukill 0:bc9f2c007928 189 pm100dx_date = month + year;
ediukill 0:bc9f2c007928 190 break;
ediukill 0:bc9f2c007928 191 case 15: // diagnostic data, 100hz , not used
ediukill 0:bc9f2c007928 192 break;
ediukill 0:bc9f2c007928 193 case 22: //parameter answer
ediukill 0:bc9f2c007928 194 break;
ediukill 0:bc9f2c007928 195 }
ediukill 0:bc9f2c007928 196 }
ediukill 0:bc9f2c007928 197
ediukill 0:bc9f2c007928 198 //return V in degC
ediukill 0:bc9f2c007928 199 float pm100dx_read_temperature(char data[]) {
ediukill 0:bc9f2c007928 200 return (float) ((int16_t) ((uint16_t) data[0] | (uint16_t) data[1] << 8))
ediukill 0:bc9f2c007928 201 / 10.0f;
ediukill 0:bc9f2c007928 202 }
ediukill 0:bc9f2c007928 203
ediukill 0:bc9f2c007928 204 //return V in volts
ediukill 0:bc9f2c007928 205 float pm100dx_read_LV(char data[]) {
ediukill 0:bc9f2c007928 206 return (float) ((int16_t) ((uint16_t) data[0] | (uint16_t) data[1] << 8))
ediukill 0:bc9f2c007928 207 / 100.0f;
ediukill 0:bc9f2c007928 208 }
ediukill 0:bc9f2c007928 209
ediukill 0:bc9f2c007928 210 //return torque in Nm
ediukill 0:bc9f2c007928 211 float pm100dx_read_Torque(char data[]) {
ediukill 0:bc9f2c007928 212 return (float) ((int16_t) ((uint16_t) data[0] | (uint16_t) data[1] << 8))
ediukill 0:bc9f2c007928 213 / 10.0f;
ediukill 0:bc9f2c007928 214 }
ediukill 0:bc9f2c007928 215
ediukill 0:bc9f2c007928 216 //return V in volts
ediukill 0:bc9f2c007928 217 float pm100dx_read_HV(char data[]) {
ediukill 0:bc9f2c007928 218 return (float) ((int16_t) ((uint16_t) data[0] | (uint16_t) data[1] << 8))
ediukill 0:bc9f2c007928 219 / 10.0f;
ediukill 0:bc9f2c007928 220 }
ediukill 0:bc9f2c007928 221
ediukill 0:bc9f2c007928 222 //in A
ediukill 0:bc9f2c007928 223 float pm100dx_read_Current(char data[]) {
ediukill 0:bc9f2c007928 224 return (float) ((int16_t) ((uint16_t) data[0] | (uint16_t) data[1] << 8))
ediukill 0:bc9f2c007928 225 / 10.0f;
ediukill 0:bc9f2c007928 226 }
ediukill 0:bc9f2c007928 227
ediukill 0:bc9f2c007928 228 //in RPM
ediukill 0:bc9f2c007928 229 float pm100dx_read_AngVel(char data[]) {
ediukill 0:bc9f2c007928 230 return (float) ((int16_t) ((uint16_t) data[0] | (uint16_t) data[1] << 8));
ediukill 0:bc9f2c007928 231 }
ediukill 0:bc9f2c007928 232
ediukill 0:bc9f2c007928 233 //in kW
ediukill 0:bc9f2c007928 234 float pm100dx_read_Power(char data[]) {
ediukill 0:bc9f2c007928 235 return (float) ((int16_t) ((uint16_t) data[0] | (uint16_t) data[1] << 8))
ediukill 0:bc9f2c007928 236 / 10.0f;
ediukill 0:bc9f2c007928 237 }
ediukill 0:bc9f2c007928 238
ediukill 0:bc9f2c007928 239 void pm100dx_init() {
ediukill 0:bc9f2c007928 240 int data[2];
ediukill 0:bc9f2c007928 241 //first disable some broadcast message we wont use
ediukill 0:bc9f2c007928 242 //disable temp2 , analog input, digital input, flux
ediukill 0:bc9f2c007928 243 data[0] = 0b11100101;
ediukill 0:bc9f2c007928 244 data[1] = 0b11111110;
ediukill 0:bc9f2c007928 245 pm100dx_command(148, data, 1);
ediukill 0:bc9f2c007928 246 }
ediukill 0:bc9f2c007928 247
ediukill 0:bc9f2c007928 248 float pm100dx_getMaxControllerT() {
ediukill 0:bc9f2c007928 249
ediukill 0:bc9f2c007928 250 float temp = 0;
ediukill 0:bc9f2c007928 251 if (pm100dx_temp_modA > temp)
ediukill 0:bc9f2c007928 252 temp = pm100dx_temp_modA;
ediukill 0:bc9f2c007928 253 if (pm100dx_temp_modB > temp)
ediukill 0:bc9f2c007928 254 temp = pm100dx_temp_modB;
ediukill 0:bc9f2c007928 255 if (pm100dx_temp_modC > temp)
ediukill 0:bc9f2c007928 256 temp = pm100dx_temp_modC;
ediukill 0:bc9f2c007928 257 if (pm100dx_temp_gate > temp)
ediukill 0:bc9f2c007928 258 temp = pm100dx_temp_gate;
ediukill 0:bc9f2c007928 259 return temp;
ediukill 0:bc9f2c007928 260 }
ediukill 0:bc9f2c007928 261
ediukill 0:bc9f2c007928 262 void pm100dx_command(int address, int p_data[], int rw) {
ediukill 0:bc9f2c007928 263 char data[8];
ediukill 0:bc9f2c007928 264 data[0] = (char) address; //paramater address
ediukill 0:bc9f2c007928 265 data[1] = (char) address >> 8; //paramater address
ediukill 0:bc9f2c007928 266 data[2] = (char) rw; //read or write
ediukill 0:bc9f2c007928 267 data[3] = 0; //reserved
ediukill 0:bc9f2c007928 268 data[4] = (char) p_data[0]; //data
ediukill 0:bc9f2c007928 269 data[5] = (char) p_data[1]; //data
ediukill 0:bc9f2c007928 270 data[6] = 0; //reserved
ediukill 0:bc9f2c007928 271 data[7] = 0; //reserved
ediukill 0:bc9f2c007928 272 pm100dx_can_tx(0X0C1 - 0x0A0 + PM100DX_CAN_BASE_ID, data);
ediukill 0:bc9f2c007928 273 }
ediukill 0:bc9f2c007928 274
ediukill 0:bc9f2c007928 275 void pm100dx_can_tx(uint32_t id, char data[]) {
ediukill 0:bc9f2c007928 276 can1.write(CANMessage(PM100DX_CAN_BASE_ID, data, 8));
ediukill 0:bc9f2c007928 277 }
ediukill 0:bc9f2c007928 278
ediukill 0:bc9f2c007928 279 void pm100dx_clear_fault() {
ediukill 0:bc9f2c007928 280 int data[2];
ediukill 0:bc9f2c007928 281 data[0] = 0;
ediukill 0:bc9f2c007928 282 data[1] = 0;
ediukill 0:bc9f2c007928 283 pm100dx_command(20, data, 1);
ediukill 0:bc9f2c007928 284 }
ediukill 0:bc9f2c007928 285
ediukill 0:bc9f2c007928 286 void pm100dx_set_torque(float torque, int dir, int enable, int discharge) {
ediukill 0:bc9f2c007928 287 char data[8];
ediukill 0:bc9f2c007928 288 int16_t itorque = (int16_t) (torque * 10.0f);
ediukill 0:bc9f2c007928 289 data[0] = (char) itorque;
ediukill 0:bc9f2c007928 290 data[1] = (char) (itorque >> 8);
ediukill 0:bc9f2c007928 291 data[2] = 0;
ediukill 0:bc9f2c007928 292 data[3] = 0;
ediukill 0:bc9f2c007928 293 data[4] = (char) dir;
ediukill 0:bc9f2c007928 294 data[5] = (char) (enable | (discharge << 1));
ediukill 0:bc9f2c007928 295 data[6] = 0;
ediukill 0:bc9f2c007928 296 data[7] = 0;
ediukill 0:bc9f2c007928 297 pm100dx_can_tx(0X0C0 - 0x0A0 + PM100DX_CAN_BASE_ID, data);
ediukill 0:bc9f2c007928 298 }
ediukill 0:bc9f2c007928 299
ediukill 0:bc9f2c007928 300
ediukill 0:bc9f2c007928 301