Diff: MITSUBA_CAN/MITSUBA_CAN.cpp

Revision:

0:f77369cabd75

--- /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
+   
+
+}