Atsushi Hattori / Mbed 2 deprecated 20210627_Logger

hattori&ide

Dependencies:   mbed

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?

UserRevisionLine numberNew 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 }