Diff: Communication/RS485/ActuatorHub/ActuatorHub.cpp

Revision:

0:669ef71cba68

Child:

11:028a150943b5

diff -r 000000000000 -r 669ef71cba68 Communication/RS485/ActuatorHub/ActuatorHub.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Communication/RS485/ActuatorHub/ActuatorHub.cpp	Sat Sep 08 06:05:22 2018 +0000
@@ -0,0 +1,186 @@
+#include "ActuatorHub.h"
+#include "mbed.h"
+
+#include "../../../CommonLibraries/RingBuffer/RingBuffer.h"
+#include "../../../System/Using.h"
+
+static char RS485Send[RS485_BUFFER_SIZE];
+static char BluetoothSend[BLUETOOTH_BUFFER_SIZE];
+
+RINGBUFFER::RingBuffer RS485SendBuffer = RINGBUFFER::RingBuffer(RS485Send, RS485_BUFFER_SIZE);
+RINGBUFFER::RingBuffer BluetoothSendBuffer = RINGBUFFER::RingBuffer(BluetoothSend, BLUETOOTH_BUFFER_SIZE);
+
+namespace ACTUATORHUB {
+    namespace MOTOR {
+        AllMotorData sendMotorData[(MOUNTING_MOTOR_NUM+12) / 13];
+
+        namespace {
+            MotorStatus motor[MOUNTING_MOTOR_NUM];
+        }
+
+        void Motor::Initialize() {
+            #if MOUNTING_MOTOR_NUM > 0
+            sendMotorData[0].direction0to3.all = 0;
+            sendMotorData[0].direction4to7.all = 0;
+            sendMotorData[0].direction8to9.all = 0;
+            #endif
+            #if MOUNTING_MOTOR_NUM > 13
+            sendMotorData[1].direction0to3.all = 0;
+            sendMotorData[1].direction4to7.all = 0;
+            sendMotorData[1].direction8to9.all = 0;
+            #endif
+            #if MOUNTING_MOTOR_NUM > 26
+            sendMotorData[2].direction0to3.all = 0;
+            sendMotorData[2].direction4to7.all = 0;
+            sendMotorData[2].direction8to9.all = 0;
+            #endif
+
+            uint8_t* pwmPo = &sendMotorData[0].pwm0;
+            for(uint8_t i=0; i<MOUNTING_MOTOR_NUM; i++) {
+                *pwmPo = 0;
+                pwmPo++;
+            }
+
+            SetDefault();
+        }
+
+        void Motor::Update(MotorStatus *status) {
+            for(uint8_t i=0; i<MOUNTING_MOTOR_NUM; i++) motor[i] = status[i];
+
+			#if MOUNTING_MOTOR_NUM > 0
+            sendMotorData[0].direction0to3.data0 = motor[0].dir;
+			sendMotorData[0].direction0to3.data1 = motor[1].dir;
+			sendMotorData[0].direction0to3.data2 = motor[2].dir;
+			sendMotorData[0].direction0to3.data3 = motor[3].dir;
+			sendMotorData[0].direction4to7.data0 = motor[4].dir;
+			sendMotorData[0].direction4to7.data1 = motor[5].dir;
+			sendMotorData[0].direction4to7.data2 = motor[6].dir;
+			sendMotorData[0].direction4to7.data3 = motor[7].dir;
+			sendMotorData[0].direction8to9.data0 = motor[8].dir;
+			sendMotorData[0].direction8to9.data1 = motor[9].dir;
+            #endif
+            #if MOUNTING_MOTOR_NUM > 13
+			sendMotorData[1].direction0to3.data0 = motor[13].dir;
+			sendMotorData[1].direction0to3.data1 = motor[14].dir;
+			sendMotorData[1].direction0to3.data2 = motor[15].dir;
+			sendMotorData[1].direction0to3.data3 = motor[16].dir;
+			sendMotorData[1].direction4to7.data0 = motor[17].dir;
+			sendMotorData[1].direction4to7.data1 = motor[18].dir;
+			sendMotorData[1].direction4to7.data2 = motor[19].dir;
+			sendMotorData[1].direction4to7.data3 = motor[20].dir;
+			sendMotorData[1].direction8to9.data0 = motor[21].dir;
+			sendMotorData[1].direction8to9.data1 = motor[22].dir;
+            #endif
+            #if MOUNTING_MOTOR_NUM > 26
+			sendMotorData[2].direction0to3.data0 = motor[26].dir;
+			sendMotorData[2].direction0to3.data1 = motor[27].dir;
+			sendMotorData[2].direction0to3.data2 = motor[28].dir;
+			sendMotorData[2].direction0to3.data3 = motor[29].dir;
+			sendMotorData[2].direction4to7.data0 = motor[30].dir;
+			sendMotorData[2].direction4to7.data1 = motor[31].dir;
+			sendMotorData[2].direction4to7.data2 = motor[32].dir;
+			sendMotorData[2].direction4to7.data3 = motor[33].dir;
+			sendMotorData[2].direction8to9.data0 = motor[34].dir;
+			sendMotorData[2].direction8to9.data1 = motor[35].dir;
+            #endif
+            
+			uint8_t* pwmPo = &sendMotorData[0].pwm0;
+			for (uint8_t i = 0;i < MOUNTING_MOTOR_NUM;i++)
+			{
+				*pwmPo = motor[i].pwm;
+				pwmPo++;
+			}
+        }
+
+        void Motor::SetDefault() {
+            for(uint8_t i=0; i<MOUNTING_MOTOR_NUM; i++) {
+                motor[i].dir = FREE;
+                motor[i].pwm = 0;
+            }
+        }
+    }
+
+    namespace SOLENOID {
+        SolenoidStatus sendSolenoidData;
+
+        void Solenoid::Initialize() {
+            sendSolenoidData.all = ALL_SOLENOID_OFF;
+        }
+
+        void Solenoid::Update(SolenoidStatus status) {
+            sendSolenoidData.all = status.all;
+        }
+    }
+
+    void ActuatorHub::Update() {
+        if(!RS485SendBuffer.InAnyData()) {
+            // __disable_irq();
+
+            #ifdef USE_MOTOR
+            #if MOUNTING_MOTOR_NUM > 0
+            RS485SendBuffer.PutData('*');
+            RS485SendBuffer.PutData(MOTOR_ADDR);
+            RS485SendBuffer.PutData(MOTOR::sendMotorData[0].direction0to3.all);
+            RS485SendBuffer.PutData(MOTOR::sendMotorData[0].direction4to7.all);
+            RS485SendBuffer.PutData(MOTOR::sendMotorData[0].direction8to9.all);
+            uint8_t* pwmPo = &MOTOR::sendMotorData[0].pwm0;
+            for(uint8_t i=0; i<13; i++) {
+                RS485SendBuffer.PutData(*pwmPo);
+                pwmPo++;
+            }
+            RS485SendBuffer.PutData('\r');
+            #endif
+
+            #if MOUNTING_MOTOR_NUM > 13
+            RS485SendBuffer.PutData('*');
+            RS485SendBuffer.PutData(MOTOR2_ADDR);
+            RS485SendBuffer.PutData(MOTOR::sendMotorData[1].direction0to3.all);
+            RS485SendBuffer.PutData(MOTOR::sendMotorData[1].direction4to7.all);
+            RS485SendBuffer.PutData(MOTOR::sendMotorData[1].direction8to9.all);
+            pwmPo = &MOTOR::sendMotorData[1].pwm0;
+            for(uint8_t i=0; i<13; i++) {
+                RS485SendBuffer.PutData(*pwmPo);
+                pwmPo++;
+            }
+            RS485SendBuffer.PutData('\r');
+            #endif
+
+            #if MOUNTING_MOTOR_NUM > 26
+            RS485SendBuffer.PutData('*');
+            RS485SendBuffer.PutData(MOTOR3_ADDR);
+            RS485SendBuffer.PutData(MOTOR::sendMotorData[2].direction0to3.all);
+            RS485SendBuffer.PutData(MOTOR::sendMotorData[2].direction4to7.all);
+            RS485SendBuffer.PutData(MOTOR::sendMotorData[2].direction8to9.all);
+            pwmPo = &MOTOR::sendMotorData[2].pwm0;
+            for(uint8_t i=0; i<(MOUNTING_MOTOR_NUM-26); i++) {
+                RS485SendBuffer.PutData(*pwmPo);
+                pwmPo++;
+            }
+            RS485SendBuffer.PutData('\r');
+            #endif
+            #endif
+
+            #ifdef USE_SOLENOID
+            RS485SendBuffer.PutData('*');
+            RS485SendBuffer.PutData(SOLENOID_ADDR);
+            RS485SendBuffer.PutData((SOLENOID::sendSolenoidData.all & 0xff00) >> 8);
+            RS485SendBuffer.PutData(SOLENOID::sendSolenoidData.all & 0x00ff);
+            RS485SendBuffer.PutData('\r');
+            #endif
+
+            #ifdef USE_BLUETOOTH
+            if(BluetoothSendBuffer.InAnyData()) {
+                RS485SendBuffer.PutData('*');
+                RS485SendBuffer.PutData(BLUETOOTH_ADDR);
+                while(BluetoothSendBuffer.InAnyData()) {
+                    RS485SendBuffer.PutData(BluetoothSendBuffer.GetData());
+                }
+                RS485SendBuffer.PutData(disconnect);
+            }
+            #endif
+
+            // __enable_irq();
+        }
+    }
+}
+