Atsushi Hattori
/
20210627_Logger
hattori&ide
Embed:
(wiki syntax)
Show/hide line numbers
MITSUBA_CAN.cpp
00001 00002 #include "mbed.h" 00003 #include "MITSUBA_CAN.h" 00004 00005 MITSUBA::MITSUBA(CAN &_can, int freq) : can(_can), canfreq(freq){ 00006 can.frequency(canfreq); 00007 } 00008 00009 void MITSUBA::GetCanData(unsigned char val){ 00010 CANMessage sendmsg; 00011 int ret = 0; 00012 00013 sendmsg.id = 0x08F89540; // Use Rear Left #1 00014 sendmsg.data[0] = val; 00015 sendmsg.len = 1; 00016 sendmsg.type = CANData; 00017 sendmsg.format = CANExtended; 00018 00019 ret = can.write(sendmsg); 00020 00021 CANMessage readmsg; 00022 00023 if(ret && can.read(readmsg)) { 00024 if(readmsg.id == 0x08850225) { 00025 // printf("Frame0\n"); 00026 ParseFrame0(readmsg.data); 00027 } 00028 if(readmsg.id == 0x08950225) { 00029 // printf("Frame1\n"); 00030 ParseFrame1(readmsg.data); 00031 } 00032 } 00033 00034 } 00035 00036 void MITSUBA::ParseFrame0(unsigned char *data){ 00037 batteryVoltage = (double)((((((unsigned int)data[1]) & 0x0003) << 8) | (((unsigned int)data[0]) & 0x00ff))) * 0.5; 00038 batteryCurrent= (double)(((((unsigned int)data[2]) & 0x0007) << 6) | ((((unsigned int)data[1]) & 0x00fc) >> 2)); 00039 // sign = ((((unsigned int)data[2]) & 0x08) >> 3) ? -1 : 1; 00040 motorCurrentPeak = (double)(((((unsigned int)data[3]) & 0x003f) << 4) | ((((unsigned int)data[2]) & 0x00f0) >> 4)); 00041 FETtemp = (double)(((((unsigned int)data[4]) & 0x0007) << 2) | ((((unsigned int)data[3]) & 0x00c0) >> 6)) * 5; 00042 rpmMotor = (double)(((((unsigned int)data[5]) & 0x007f) << 5) | ((((unsigned int)data[4]) & 0x00f8) >> 3)); 00043 duty = (double)(((((unsigned int)data[5]) & 0x0080) >> 7) | ((((unsigned int)data[6]) & 0x00ff) << 1) | ((((unsigned int)data[7]) & 0x0001) << 9)) * 0.5; 00044 angle = (double)((((unsigned int)data[7]) & 0x00fe) >> 1) * 0.5; 00045 } 00046 00047 void MITSUBA::ParseFrame1(unsigned char *data){ 00048 00049 /* 00050 PEmode = (int)(data[0] & 0x01); //Power Eco mode 00051 Contmode = (int)((data[0] & 0x02) >> 1); //Motor Control mode 00052 */ 00053 accelPosition = (double)(((((unsigned int)data[0]) & 0x00fc) >> 2) | ((((unsigned int)data[1]) & 0x000f) << 6))*0.5; //0.5%/LSB Accelerator Position 00054 regenePosition = (double)(((((unsigned int)data[1]) & 0x00f0) >> 4) | ((((unsigned int)data[2]) & 0x003f) << 4))*0.5; //0.5%/LSB Regeneration Position 00055 00056 // DigiSWposition = (int)(((((unsigned int)data[2]) & 0x00c0) >> 6) | ((((unsigned int)data[3]) & 0x0003) << 2)); //Digi SW number 00057 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 00058 // Status = (int)((data[4] & 0x30) >> 4); //Drive Action Status 00059 // drivemode = (int)((data[4] & 0x40) >> 6); //Regeneration Status 00060 00061 00062 }
Generated on Sun Dec 18 2022 08:16:46 by 1.7.2