Diff: I2C_COM/I2C_master.cpp

Revision:

6:584653235830

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/I2C_COM/I2C_master.cpp	Tue Sep 14 11:18:35 2021 +0000
@@ -0,0 +1,63 @@
+#include "I2C_master.h"
+
+I2C_master::I2C_master(PinName sda, PinName scl, int addr){  
+    i2c = new I2C(sda, scl);
+    i2c->frequency(100000);
+    messageSent = true;
+    address = addr;
+    buf = new char[sizeof(I2cResponseMsg)];
+
+}
+
+uint8_t I2C_master::CheckSumFun(uint8_t* byteData, int length)
+{
+    uint8_t chkSumByte = 0x00;
+    //printf("lenght: %d\n", length);
+    for (int i = 0; i < length; i++)
+        chkSumByte ^= byteData[i];
+    return chkSumByte;
+}
+
+void I2C_master::internal_cb_handler(int event) {
+    if (event & I2C_EVENT_ERROR_NO_SLAVE) {
+        //printf("not responding\n");
+
+    } else if (event & I2C_EVENT_ERROR) {
+        //printf("error\n");
+
+    } else {
+        //printf("sending done\n");
+        //done_cb.call(123);
+        messageSent = true;
+    }
+}
+
+bool I2C_master::updateI2CCommunication(I2cRobotCmd _cmd, float _clampPposition, float _wheelsSpeed, float &wheelsPos_dx, float &wheelsPos_sx, float &wheelsSpeed_dx, float &wheelsSpeed_sx ) {
+    
+    if(messageSent == true){
+    
+        I2cResponseMsg *toolRsp_;
+    
+        toolCmd.Cmd = _cmd;
+        toolCmd.Data.clampPposition = _clampPposition;
+        toolCmd.Data.wheelsSpeed = _wheelsSpeed;
+        toolCmd.Checksum = 255;
+        uint8_t cks = CheckSumFun((uint8_t*)&toolCmd, sizeof(I2cComandMsg));
+        toolCmd.Checksum = cks;
+        i2c->transfer(address, (char*)&toolCmd, sizeof(I2cComandMsg), buf, sizeof(I2cResponseMsg), event_callback_t(this, &I2C_master::internal_cb_handler), I2C_EVENT_ALL, true);
+
+        toolRsp_ = (I2cResponseMsg *)buf;
+
+        wheelsPos_dx = toolRsp_->Data.wheelsPos_dx; 
+        wheelsPos_sx = toolRsp_->Data.wheelsPos_sx; 
+        wheelsSpeed_dx = toolRsp_->Data.wheelsSpeed_dx; 
+        wheelsSpeed_sx = toolRsp_->Data.wheelsSpeed_sx; 
+
+        messageSent = false;
+        return true;
+    }else{
+        return false;
+    }
+
+}
+