Atsushi Hattori
/
20210627_Logger
hattori&ide
MITSUBA_CAN/MITSUBA_CAN.cpp@0:f77369cabd75, 18 months ago (annotated)
- Committer:
- hattori_atsushi
- Date:
- Sun Dec 18 08:16:01 2022 +0000
- Revision:
- 0:f77369cabd75
hattori
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
hattori_atsushi | 0:f77369cabd75 | 1 | |
hattori_atsushi | 0:f77369cabd75 | 2 | #include "mbed.h" |
hattori_atsushi | 0:f77369cabd75 | 3 | #include "MITSUBA_CAN.h" |
hattori_atsushi | 0:f77369cabd75 | 4 | |
hattori_atsushi | 0:f77369cabd75 | 5 | MITSUBA::MITSUBA(CAN &_can, int freq) : can(_can), canfreq(freq){ |
hattori_atsushi | 0:f77369cabd75 | 6 | can.frequency(canfreq); |
hattori_atsushi | 0:f77369cabd75 | 7 | } |
hattori_atsushi | 0:f77369cabd75 | 8 | |
hattori_atsushi | 0:f77369cabd75 | 9 | void MITSUBA::GetCanData(unsigned char val){ |
hattori_atsushi | 0:f77369cabd75 | 10 | CANMessage sendmsg; |
hattori_atsushi | 0:f77369cabd75 | 11 | int ret = 0; |
hattori_atsushi | 0:f77369cabd75 | 12 | |
hattori_atsushi | 0:f77369cabd75 | 13 | sendmsg.id = 0x08F89540; // Use Rear Left #1 |
hattori_atsushi | 0:f77369cabd75 | 14 | sendmsg.data[0] = val; |
hattori_atsushi | 0:f77369cabd75 | 15 | sendmsg.len = 1; |
hattori_atsushi | 0:f77369cabd75 | 16 | sendmsg.type = CANData; |
hattori_atsushi | 0:f77369cabd75 | 17 | sendmsg.format = CANExtended; |
hattori_atsushi | 0:f77369cabd75 | 18 | |
hattori_atsushi | 0:f77369cabd75 | 19 | ret = can.write(sendmsg); |
hattori_atsushi | 0:f77369cabd75 | 20 | |
hattori_atsushi | 0:f77369cabd75 | 21 | CANMessage readmsg; |
hattori_atsushi | 0:f77369cabd75 | 22 | |
hattori_atsushi | 0:f77369cabd75 | 23 | if(ret && can.read(readmsg)) { |
hattori_atsushi | 0:f77369cabd75 | 24 | if(readmsg.id == 0x08850225) { |
hattori_atsushi | 0:f77369cabd75 | 25 | // printf("Frame0\n"); |
hattori_atsushi | 0:f77369cabd75 | 26 | ParseFrame0(readmsg.data); |
hattori_atsushi | 0:f77369cabd75 | 27 | } |
hattori_atsushi | 0:f77369cabd75 | 28 | if(readmsg.id == 0x08950225) { |
hattori_atsushi | 0:f77369cabd75 | 29 | // printf("Frame1\n"); |
hattori_atsushi | 0:f77369cabd75 | 30 | ParseFrame1(readmsg.data); |
hattori_atsushi | 0:f77369cabd75 | 31 | } |
hattori_atsushi | 0:f77369cabd75 | 32 | } |
hattori_atsushi | 0:f77369cabd75 | 33 | |
hattori_atsushi | 0:f77369cabd75 | 34 | } |
hattori_atsushi | 0:f77369cabd75 | 35 | |
hattori_atsushi | 0:f77369cabd75 | 36 | void MITSUBA::ParseFrame0(unsigned char *data){ |
hattori_atsushi | 0:f77369cabd75 | 37 | batteryVoltage = (double)((((((unsigned int)data[1]) & 0x0003) << 8) | (((unsigned int)data[0]) & 0x00ff))) * 0.5; |
hattori_atsushi | 0:f77369cabd75 | 38 | batteryCurrent= (double)(((((unsigned int)data[2]) & 0x0007) << 6) | ((((unsigned int)data[1]) & 0x00fc) >> 2)); |
hattori_atsushi | 0:f77369cabd75 | 39 | // sign = ((((unsigned int)data[2]) & 0x08) >> 3) ? -1 : 1; |
hattori_atsushi | 0:f77369cabd75 | 40 | motorCurrentPeak = (double)(((((unsigned int)data[3]) & 0x003f) << 4) | ((((unsigned int)data[2]) & 0x00f0) >> 4)); |
hattori_atsushi | 0:f77369cabd75 | 41 | FETtemp = (double)(((((unsigned int)data[4]) & 0x0007) << 2) | ((((unsigned int)data[3]) & 0x00c0) >> 6)) * 5; |
hattori_atsushi | 0:f77369cabd75 | 42 | rpmMotor = (double)(((((unsigned int)data[5]) & 0x007f) << 5) | ((((unsigned int)data[4]) & 0x00f8) >> 3)); |
hattori_atsushi | 0:f77369cabd75 | 43 | duty = (double)(((((unsigned int)data[5]) & 0x0080) >> 7) | ((((unsigned int)data[6]) & 0x00ff) << 1) | ((((unsigned int)data[7]) & 0x0001) << 9)) * 0.5; |
hattori_atsushi | 0:f77369cabd75 | 44 | angle = (double)((((unsigned int)data[7]) & 0x00fe) >> 1) * 0.5; |
hattori_atsushi | 0:f77369cabd75 | 45 | } |
hattori_atsushi | 0:f77369cabd75 | 46 | |
hattori_atsushi | 0:f77369cabd75 | 47 | void MITSUBA::ParseFrame1(unsigned char *data){ |
hattori_atsushi | 0:f77369cabd75 | 48 | |
hattori_atsushi | 0:f77369cabd75 | 49 | /* |
hattori_atsushi | 0:f77369cabd75 | 50 | PEmode = (int)(data[0] & 0x01); //Power Eco mode |
hattori_atsushi | 0:f77369cabd75 | 51 | Contmode = (int)((data[0] & 0x02) >> 1); //Motor Control mode |
hattori_atsushi | 0:f77369cabd75 | 52 | */ |
hattori_atsushi | 0:f77369cabd75 | 53 | accelPosition = (double)(((((unsigned int)data[0]) & 0x00fc) >> 2) | ((((unsigned int)data[1]) & 0x000f) << 6))*0.5; //0.5%/LSB Accelerator Position |
hattori_atsushi | 0:f77369cabd75 | 54 | regenePosition = (double)(((((unsigned int)data[1]) & 0x00f0) >> 4) | ((((unsigned int)data[2]) & 0x003f) << 4))*0.5; //0.5%/LSB Regeneration Position |
hattori_atsushi | 0:f77369cabd75 | 55 | |
hattori_atsushi | 0:f77369cabd75 | 56 | // DigiSWposition = (int)(((((unsigned int)data[2]) & 0x00c0) >> 6) | ((((unsigned int)data[3]) & 0x0003) << 2)); //Digi SW number |
hattori_atsushi | 0:f77369cabd75 | 57 | OUTtarget = (double)(((((unsigned int)data[3]) & 0x00fc) >> 2) | ((((unsigned int)data[4]) & 0x000f) << 6))*0.5; //0.5%/LSB or 0.5A/LSB Output Target Value |
hattori_atsushi | 0:f77369cabd75 | 58 | // Status = (int)((data[4] & 0x30) >> 4); //Drive Action Status |
hattori_atsushi | 0:f77369cabd75 | 59 | // drivemode = (int)((data[4] & 0x40) >> 6); //Regeneration Status |
hattori_atsushi | 0:f77369cabd75 | 60 | |
hattori_atsushi | 0:f77369cabd75 | 61 | |
hattori_atsushi | 0:f77369cabd75 | 62 | } |