lknds
Dependencies: mbed TrapezoidControl Pulse QEI
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 RS485SendBuffer.PutData('*'); 00184 RS485SendBuffer.PutData(TAPELED_ADDR); 00185 RS485SendBuffer.PutData(sendLedData.red); 00186 RS485SendBuffer.PutData(sendLedData.green); 00187 RS485SendBuffer.PutData(sendLedData.blue); 00188 RS485SendBuffer.PutData('\r'); 00189 00190 __enable_irq(); 00191 } 00192 } 00193 } 00194
Generated on Sun Jul 17 2022 15:33:12 by 1.7.2