ActuatorHub.cpp

00001 #include "ActuatorHub.h"
00002 #include "mbed.h"
00003 
00004 #include "../../../CommonLibraries/RingBuffer/RingBuffer.h"
00005 #include "../../../System/Process/Process.h"
00006 #include "../../../System/Using.h"
00007 
00008 static char RS485Send[RS485_BUFFER_SIZE];
00009 static char BluetoothSend[BLUETOOTH_BUFFER_SIZE];
00010 
00011 RINGBUFFER::RingBuffer RS485SendBuffer = RINGBUFFER::RingBuffer(RS485Send, RS485_BUFFER_SIZE);
00012 RINGBUFFER::RingBuffer BluetoothSendBuffer = RINGBUFFER::RingBuffer(BluetoothSend, BLUETOOTH_BUFFER_SIZE);
00013 
00014 namespace ACTUATORHUB {
00015     namespace MOTOR {
00016         AllMotorData sendMotorData[(MOUNTING_MOTOR_NUM+12) / 13];
00017 
00018         namespace {
00019             MotorStatus motor[MOUNTING_MOTOR_NUM];
00020         }
00021 
00022         void Motor::Initialize() {
00023             #if MOUNTING_MOTOR_NUM > 0
00024             sendMotorData[0].direction0to3.all = 0;
00025             sendMotorData[0].direction4to7.all = 0;
00026             sendMotorData[0].direction8to9.all = 0;
00027             #endif
00028             #if MOUNTING_MOTOR_NUM > 13
00029             sendMotorData[1].direction0to3.all = 0;
00030             sendMotorData[1].direction4to7.all = 0;
00031             sendMotorData[1].direction8to9.all = 0;
00032             #endif
00033             #if MOUNTING_MOTOR_NUM > 26
00034             sendMotorData[2].direction0to3.all = 0;
00035             sendMotorData[2].direction4to7.all = 0;
00036             sendMotorData[2].direction8to9.all = 0;
00037             #endif
00038 
00039             uint8_t* pwmPo = &sendMotorData[0].pwm0;
00040             for(uint8_t i=0; i<MOUNTING_MOTOR_NUM; i++) {
00041                 *pwmPo = 0;
00042                 pwmPo++;
00043             }
00044 
00045             SetDefault();
00046         }
00047 
00048         void Motor::Update(MotorStatus *status) {
00049             for(uint8_t i=0; i<MOUNTING_MOTOR_NUM; i++) motor[i] = status[i];
00050 
00051             #if MOUNTING_MOTOR_NUM > 0
00052             sendMotorData[0].direction0to3.data0 = motor[0].dir;
00053             sendMotorData[0].direction0to3.data1 = motor[1].dir;
00054             sendMotorData[0].direction0to3.data2 = motor[2].dir;
00055             sendMotorData[0].direction0to3.data3 = motor[3].dir;
00056             sendMotorData[0].direction4to7.data0 = motor[4].dir;
00057             sendMotorData[0].direction4to7.data1 = motor[5].dir;
00058             sendMotorData[0].direction4to7.data2 = motor[6].dir;
00059             sendMotorData[0].direction4to7.data3 = motor[7].dir;
00060             sendMotorData[0].direction8to9.data0 = motor[8].dir;
00061             sendMotorData[0].direction8to9.data1 = motor[9].dir;
00062             #endif
00063             #if MOUNTING_MOTOR_NUM > 13
00064             sendMotorData[1].direction0to3.data0 = motor[13].dir;
00065             sendMotorData[1].direction0to3.data1 = motor[14].dir;
00066             sendMotorData[1].direction0to3.data2 = motor[15].dir;
00067             sendMotorData[1].direction0to3.data3 = motor[16].dir;
00068             sendMotorData[1].direction4to7.data0 = motor[17].dir;
00069             sendMotorData[1].direction4to7.data1 = motor[18].dir;
00070             sendMotorData[1].direction4to7.data2 = motor[19].dir;
00071             sendMotorData[1].direction4to7.data3 = motor[20].dir;
00072             sendMotorData[1].direction8to9.data0 = motor[21].dir;
00073             sendMotorData[1].direction8to9.data1 = motor[22].dir;
00074             #endif
00075             #if MOUNTING_MOTOR_NUM > 26
00076             sendMotorData[2].direction0to3.data0 = motor[26].dir;
00077             sendMotorData[2].direction0to3.data1 = motor[27].dir;
00078             sendMotorData[2].direction0to3.data2 = motor[28].dir;
00079             sendMotorData[2].direction0to3.data3 = motor[29].dir;
00080             sendMotorData[2].direction4to7.data0 = motor[30].dir;
00081             sendMotorData[2].direction4to7.data1 = motor[31].dir;
00082             sendMotorData[2].direction4to7.data2 = motor[32].dir;
00083             sendMotorData[2].direction4to7.data3 = motor[33].dir;
00084             sendMotorData[2].direction8to9.data0 = motor[34].dir;
00085             sendMotorData[2].direction8to9.data1 = motor[35].dir;
00086             #endif
00087             
00088             uint8_t* pwmPo = &sendMotorData[0].pwm0;
00089             for (uint8_t i = 0;i < MOUNTING_MOTOR_NUM;i++)
00090             {
00091                 *pwmPo = motor[i].pwm;
00092                 pwmPo++;
00093             }
00094         }
00095 
00096         void Motor::SetDefault() {
00097             for(uint8_t i=0; i<MOUNTING_MOTOR_NUM; i++) {
00098                 motor[i].dir = FREE;
00099                 motor[i].pwm = 0;
00100             }
00101         }
00102     }
00103 
00104     namespace SOLENOID {
00105         SolenoidStatus sendSolenoidData;
00106 
00107         void Solenoid::Initialize() {
00108             sendSolenoidData.all = ALL_SOLENOID_OFF;
00109         }
00110 
00111         void Solenoid::Update(SolenoidStatus status) {
00112             sendSolenoidData.all = status.all;
00113         }
00114     }
00115 
00116     void ActuatorHub::Update() {
00117         if(!RS485SendBuffer.InAnyData()) {
00118             // __disable_irq();
00119 
00120             #ifdef USE_MOTOR
00121             #if MOUNTING_MOTOR_NUM > 0
00122             RS485SendBuffer.PutData('*');
00123             RS485SendBuffer.PutData(MOTOR_ADDR);
00124             RS485SendBuffer.PutData(MOTOR::sendMotorData[0].direction0to3.all);
00125             RS485SendBuffer.PutData(MOTOR::sendMotorData[0].direction4to7.all);
00126             RS485SendBuffer.PutData(MOTOR::sendMotorData[0].direction8to9.all);
00127             uint8_t* pwmPo = &MOTOR::sendMotorData[0].pwm0;
00128             for(uint8_t i=0; i<13; i++) {
00129                 RS485SendBuffer.PutData(*pwmPo);
00130                 pwmPo++;
00131             }
00132             RS485SendBuffer.PutData('\r');
00133             #endif
00134 
00135             #if MOUNTING_MOTOR_NUM > 13
00136             RS485SendBuffer.PutData('*');
00137             RS485SendBuffer.PutData(MOTOR2_ADDR);
00138             RS485SendBuffer.PutData(MOTOR::sendMotorData[1].direction0to3.all);
00139             RS485SendBuffer.PutData(MOTOR::sendMotorData[1].direction4to7.all);
00140             RS485SendBuffer.PutData(MOTOR::sendMotorData[1].direction8to9.all);
00141             pwmPo = &MOTOR::sendMotorData[1].pwm0;
00142             for(uint8_t i=0; i<13; i++) {
00143                 RS485SendBuffer.PutData(*pwmPo);
00144                 pwmPo++;
00145             }
00146             RS485SendBuffer.PutData('\r');
00147             #endif
00148 
00149             #if MOUNTING_MOTOR_NUM > 26
00150             RS485SendBuffer.PutData('*');
00151             RS485SendBuffer.PutData(MOTOR3_ADDR);
00152             RS485SendBuffer.PutData(MOTOR::sendMotorData[2].direction0to3.all);
00153             RS485SendBuffer.PutData(MOTOR::sendMotorData[2].direction4to7.all);
00154             RS485SendBuffer.PutData(MOTOR::sendMotorData[2].direction8to9.all);
00155             pwmPo = &MOTOR::sendMotorData[2].pwm0;
00156             for(uint8_t i=0; i<(MOUNTING_MOTOR_NUM-26); i++) {
00157                 RS485SendBuffer.PutData(*pwmPo);
00158                 pwmPo++;
00159             }
00160             RS485SendBuffer.PutData('\r');
00161             #endif
00162             #endif
00163 
00164             #ifdef USE_SOLENOID
00165             RS485SendBuffer.PutData('*');
00166             RS485SendBuffer.PutData(SOLENOID_ADDR);
00167             RS485SendBuffer.PutData((SOLENOID::sendSolenoidData.all & 0xff00) >> 8);
00168             RS485SendBuffer.PutData(SOLENOID::sendSolenoidData.all & 0x00ff);
00169             RS485SendBuffer.PutData('\r');
00170             #endif
00171 
00172             #ifdef USE_BLUETOOTH
00173             if(BluetoothSendBuffer.InAnyData()) {
00174                 RS485SendBuffer.PutData('*');
00175                 RS485SendBuffer.PutData(BLUETOOTH_ADDR);
00176                 while(BluetoothSendBuffer.InAnyData()) {
00177                     RS485SendBuffer.PutData(BluetoothSendBuffer.GetData());
00178                 }
00179                 RS485SendBuffer.PutData(disconnect);
00180             }
00181             #endif
00182 
00183             __enable_irq();
00184         }
00185     }
00186 }
00187