Nagano kosen robocon

Dependencies:   mbed QEI

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers ActuatorHub.cpp Source File

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 HubRS485Send[HUB_RS485_BUFFER_SIZE];
00009 static char BluetoothSend[BLUETOOTH_BUFFER_SIZE];
00010 
00011 RINGBUFFER::RingBuffer HubRS485SendBuffer = RINGBUFFER::RingBuffer(HubRS485Send, HUB_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                 if(i==12) pwmPo = &sendMotorData[1].pwm0;
00043                 else if(i==25) pwmPo = &sendMotorData[2].pwm0;
00044                 else pwmPo++;
00045             }
00046 
00047             SetDefault();
00048         }
00049 
00050         void Motor::Update(MotorStatus *status) {
00051             for(uint8_t i=0; i<MOUNTING_MOTOR_NUM; i++) motor[i] = status[i];
00052 
00053             #if MOUNTING_MOTOR_NUM > 0
00054             sendMotorData[0].direction0to3.data0 = motor[0].dir;
00055             sendMotorData[0].direction0to3.data1 = motor[1].dir;
00056             sendMotorData[0].direction0to3.data2 = motor[2].dir;
00057             sendMotorData[0].direction0to3.data3 = motor[3].dir;
00058             sendMotorData[0].direction4to7.data0 = motor[4].dir;
00059             sendMotorData[0].direction4to7.data1 = motor[5].dir;
00060             sendMotorData[0].direction4to7.data2 = motor[6].dir;
00061             sendMotorData[0].direction4to7.data3 = motor[7].dir;
00062             sendMotorData[0].direction8to9.data0 = motor[8].dir;
00063             sendMotorData[0].direction8to9.data1 = motor[9].dir;
00064             #endif
00065             #if MOUNTING_MOTOR_NUM > 13
00066             sendMotorData[1].direction0to3.data0 = motor[13].dir;
00067             sendMotorData[1].direction0to3.data1 = motor[14].dir;
00068             sendMotorData[1].direction0to3.data2 = motor[15].dir;
00069             sendMotorData[1].direction0to3.data3 = motor[16].dir;
00070             sendMotorData[1].direction4to7.data0 = motor[17].dir;
00071             sendMotorData[1].direction4to7.data1 = motor[18].dir;
00072             sendMotorData[1].direction4to7.data2 = motor[19].dir;
00073             sendMotorData[1].direction4to7.data3 = motor[20].dir;
00074             sendMotorData[1].direction8to9.data0 = motor[21].dir;
00075             sendMotorData[1].direction8to9.data1 = motor[22].dir;
00076             #endif
00077             #if MOUNTING_MOTOR_NUM > 26
00078             sendMotorData[2].direction0to3.data0 = motor[26].dir;
00079             sendMotorData[2].direction0to3.data1 = motor[27].dir;
00080             sendMotorData[2].direction0to3.data2 = motor[28].dir;
00081             sendMotorData[2].direction0to3.data3 = motor[29].dir;
00082             sendMotorData[2].direction4to7.data0 = motor[30].dir;
00083             sendMotorData[2].direction4to7.data1 = motor[31].dir;
00084             sendMotorData[2].direction4to7.data2 = motor[32].dir;
00085             sendMotorData[2].direction4to7.data3 = motor[33].dir;
00086             sendMotorData[2].direction8to9.data0 = motor[34].dir;
00087             sendMotorData[2].direction8to9.data1 = motor[35].dir;
00088             #endif
00089             
00090             uint8_t* pwmPo = &sendMotorData[0].pwm0;
00091             for (uint8_t i = 0;i < MOUNTING_MOTOR_NUM;i++)
00092             {
00093                 *pwmPo = motor[i].pwm;
00094                 if(i==12) pwmPo = &sendMotorData[1].pwm0;
00095                 else if(i==25) pwmPo = &sendMotorData[2].pwm0;
00096                 else pwmPo++;
00097             }
00098         }
00099 
00100         void Motor::SetDefault() {
00101             for(uint8_t i=0; i<MOUNTING_MOTOR_NUM; i++) {
00102                 motor[i].dir = FREE;
00103                 motor[i].pwm = 0;
00104             }
00105         }
00106     }
00107 
00108     namespace SOLENOID {
00109         SolenoidStatus sendSolenoidData;
00110 
00111         void Solenoid::Initialize() {
00112             sendSolenoidData.all = ALL_SOLENOID_OFF;
00113         }
00114 
00115         void Solenoid::Update(SolenoidStatus status) {
00116             sendSolenoidData.all = status.all;
00117         }
00118     }
00119 
00120     void ActuatorHub::Update() {
00121         if(!HubRS485SendBuffer.InAnyData()) {
00122             __disable_irq();
00123 
00124             #ifdef USE_MOTOR
00125             #if MOUNTING_MOTOR_NUM > 0
00126             HubRS485SendBuffer.PutData('*');
00127             HubRS485SendBuffer.PutData(MOTOR_ADDR);
00128             HubRS485SendBuffer.PutData(MOTOR::sendMotorData[0].direction0to3.all);
00129             HubRS485SendBuffer.PutData(MOTOR::sendMotorData[0].direction4to7.all);
00130             HubRS485SendBuffer.PutData(MOTOR::sendMotorData[0].direction8to9.all);
00131             uint8_t* pwmPo = &MOTOR::sendMotorData[0].pwm0;
00132             for(uint8_t i=0; i<13; i++) {
00133                 HubRS485SendBuffer.PutData(*pwmPo);
00134                 pwmPo++;
00135             }
00136             HubRS485SendBuffer.PutData('\r');
00137             #endif
00138 
00139             #if MOUNTING_MOTOR_NUM > 13
00140             HubRS485SendBuffer.PutData('*');
00141             HubRS485SendBuffer.PutData(MOTOR2_ADDR);
00142             HubRS485SendBuffer.PutData(MOTOR::sendMotorData[1].direction0to3.all);
00143             HubRS485SendBuffer.PutData(MOTOR::sendMotorData[1].direction4to7.all);
00144             HubRS485SendBuffer.PutData(MOTOR::sendMotorData[1].direction8to9.all);
00145             pwmPo = &MOTOR::sendMotorData[1].pwm0;
00146             for(uint8_t i=0; i<13; i++) {
00147                 HubHubRS485SendBuffer.PutData(*pwmPo);
00148                 pwmPo++;
00149             }
00150             HubRS485SendBuffer.PutData('\r');
00151             #endif
00152 
00153             #if MOUNTING_MOTOR_NUM > 26
00154             HubRS485SendBuffer.PutData('*');
00155             HubRS485SendBuffer.PutData(MOTOR3_ADDR);
00156             HubRS485SendBuffer.PutData(MOTOR::sendMotorData[2].direction0to3.all);
00157             HubRS485SendBuffer.PutData(MOTOR::sendMotorData[2].direction4to7.all);
00158             HubRS485SendBuffer.PutData(MOTOR::sendMotorData[2].direction8to9.all);
00159             pwmPo = &MOTOR::sendMotorData[2].pwm0;
00160             for(uint8_t i=0; i<(MOUNTING_MOTOR_NUM-26); i++) {
00161                 HubHubRS485SendBuffer.PutData(*pwmPo);
00162                 pwmPo++;
00163             }
00164             HubRS485SendBuffer.PutData('\r');
00165             #endif
00166             #endif
00167             /*
00168             #ifdef USE_SOLENOID
00169             HubRS485SendBuffer.PutData('*');
00170             HubRS485SendBuffer.PutData(SOLENOID_ADDR);
00171             HubRS485SendBuffer.PutData((SOLENOID::sendSolenoidData.all & 0xff00) >> 8);
00172             HubRS485SendBuffer.PutData(SOLENOID::sendSolenoidData.all & 0x00ff);
00173             HubRS485SendBuffer.PutData('\r');
00174             #endif
00175 
00176             #ifdef USE_BLUETOOTH
00177             if(BluetoothSendBuffer.InAnyData()) {
00178                 HubHubRS485SendBuffer.PutData('*');
00179                 HubHubRS485SendBuffer.PutData(BLUETOOTH_ADDR);
00180                 while(BluetoothSendBuffer.InAnyData()) {
00181                     HubHubRS485SendBuffer.PutData(BluetoothSendBuffer.GetData());
00182                 }
00183                 HubHubRS485SendBuffer.PutData(disconnect);
00184             }
00185             #endif
00186             */
00187 
00188             __enable_irq();
00189         }
00190     }
00191 }