Atsushi Hattori
/
20210627_Logger
hattori&ide
Diff: MITSUBA_CAN/MITSUBA_CAN.cpp
- Revision:
- 0:f77369cabd75
diff -r 000000000000 -r f77369cabd75 MITSUBA_CAN/MITSUBA_CAN.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MITSUBA_CAN/MITSUBA_CAN.cpp Sun Dec 18 08:16:01 2022 +0000 @@ -0,0 +1,62 @@ + +#include "mbed.h" +#include "MITSUBA_CAN.h" + +MITSUBA::MITSUBA(CAN &_can, int freq) : can(_can), canfreq(freq){ + can.frequency(canfreq); +} + +void MITSUBA::GetCanData(unsigned char val){ + CANMessage sendmsg; + int ret = 0; + + sendmsg.id = 0x08F89540; // Use Rear Left #1 + sendmsg.data[0] = val; + sendmsg.len = 1; + sendmsg.type = CANData; + sendmsg.format = CANExtended; + + ret = can.write(sendmsg); + + CANMessage readmsg; + + if(ret && can.read(readmsg)) { + if(readmsg.id == 0x08850225) { + // printf("Frame0\n"); + ParseFrame0(readmsg.data); + } + if(readmsg.id == 0x08950225) { + // printf("Frame1\n"); + ParseFrame1(readmsg.data); + } + } + +} + +void MITSUBA::ParseFrame0(unsigned char *data){ + batteryVoltage = (double)((((((unsigned int)data[1]) & 0x0003) << 8) | (((unsigned int)data[0]) & 0x00ff))) * 0.5; + batteryCurrent= (double)(((((unsigned int)data[2]) & 0x0007) << 6) | ((((unsigned int)data[1]) & 0x00fc) >> 2)); + // sign = ((((unsigned int)data[2]) & 0x08) >> 3) ? -1 : 1; + motorCurrentPeak = (double)(((((unsigned int)data[3]) & 0x003f) << 4) | ((((unsigned int)data[2]) & 0x00f0) >> 4)); + FETtemp = (double)(((((unsigned int)data[4]) & 0x0007) << 2) | ((((unsigned int)data[3]) & 0x00c0) >> 6)) * 5; + rpmMotor = (double)(((((unsigned int)data[5]) & 0x007f) << 5) | ((((unsigned int)data[4]) & 0x00f8) >> 3)); + duty = (double)(((((unsigned int)data[5]) & 0x0080) >> 7) | ((((unsigned int)data[6]) & 0x00ff) << 1) | ((((unsigned int)data[7]) & 0x0001) << 9)) * 0.5; + angle = (double)((((unsigned int)data[7]) & 0x00fe) >> 1) * 0.5; +} + +void MITSUBA::ParseFrame1(unsigned char *data){ + + /* + PEmode = (int)(data[0] & 0x01); //Power Eco mode + Contmode = (int)((data[0] & 0x02) >> 1); //Motor Control mode + */ + accelPosition = (double)(((((unsigned int)data[0]) & 0x00fc) >> 2) | ((((unsigned int)data[1]) & 0x000f) << 6))*0.5; //0.5%/LSB Accelerator Position + regenePosition = (double)(((((unsigned int)data[1]) & 0x00f0) >> 4) | ((((unsigned int)data[2]) & 0x003f) << 4))*0.5; //0.5%/LSB Regeneration Position + + // DigiSWposition = (int)(((((unsigned int)data[2]) & 0x00c0) >> 6) | ((((unsigned int)data[3]) & 0x0003) << 2)); //Digi SW number + 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 + // Status = (int)((data[4] & 0x30) >> 4); //Drive Action Status + // drivemode = (int)((data[4] & 0x40) >> 6); //Regeneration Status + + +}