File content as of revision 0:669ef71cba68:

#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();
        }
    }
}