Miya Miyagawa / Mbed 2 deprecated AutoMaster_si-tu

lknds

Dependencies:   mbed TrapezoidControl Pulse QEI

Communication/RS485/ActuatorHub/ActuatorHub.cpp@25:d367d1e7a153, 2019-09-27 (annotated)

Committer:
Ryosei
Date:
Fri Sep 27 05:19:58 2019 +0000
Revision:
25:d367d1e7a153
Parent:
11:028a150943b5 
dskb

Who changed what in which revision?

UserRevisionLine numberNew contents of line
t_yamamoto 0:669ef71cba68 1 #include "ActuatorHub.h"
t_yamamoto 0:669ef71cba68 2 #include "mbed.h"
t_yamamoto 0:669ef71cba68 3
t_yamamoto 0:669ef71cba68 4 #include "../../../CommonLibraries/RingBuffer/RingBuffer.h"
kishibekairohan 11:028a150943b5 5 #include "../../../System/Process/Process.h"
t_yamamoto 0:669ef71cba68 6 #include "../../../System/Using.h"
t_yamamoto 0:669ef71cba68 7
t_yamamoto 0:669ef71cba68 8 static char RS485Send[RS485_BUFFER_SIZE];
t_yamamoto 0:669ef71cba68 9 static char BluetoothSend[BLUETOOTH_BUFFER_SIZE];
t_yamamoto 0:669ef71cba68 10
t_yamamoto 0:669ef71cba68 11 RINGBUFFER::RingBuffer RS485SendBuffer = RINGBUFFER::RingBuffer(RS485Send, RS485_BUFFER_SIZE);
t_yamamoto 0:669ef71cba68 12 RINGBUFFER::RingBuffer BluetoothSendBuffer = RINGBUFFER::RingBuffer(BluetoothSend, BLUETOOTH_BUFFER_SIZE);
t_yamamoto 0:669ef71cba68 13
t_yamamoto 0:669ef71cba68 14 namespace ACTUATORHUB {
t_yamamoto 0:669ef71cba68 15 namespace MOTOR {
t_yamamoto 0:669ef71cba68 16 AllMotorData sendMotorData[(MOUNTING_MOTOR_NUM+12) / 13];
t_yamamoto 0:669ef71cba68 17
t_yamamoto 0:669ef71cba68 18 namespace {
t_yamamoto 0:669ef71cba68 19 MotorStatus motor[MOUNTING_MOTOR_NUM];
t_yamamoto 0:669ef71cba68 20 }
t_yamamoto 0:669ef71cba68 21
t_yamamoto 0:669ef71cba68 22 void Motor::Initialize() {
t_yamamoto 0:669ef71cba68 23 #if MOUNTING_MOTOR_NUM > 0
t_yamamoto 0:669ef71cba68 24 sendMotorData[0].direction0to3.all = 0;
t_yamamoto 0:669ef71cba68 25 sendMotorData[0].direction4to7.all = 0;
t_yamamoto 0:669ef71cba68 26 sendMotorData[0].direction8to9.all = 0;
t_yamamoto 0:669ef71cba68 27 #endif
t_yamamoto 0:669ef71cba68 28 #if MOUNTING_MOTOR_NUM > 13
t_yamamoto 0:669ef71cba68 29 sendMotorData[1].direction0to3.all = 0;
t_yamamoto 0:669ef71cba68 30 sendMotorData[1].direction4to7.all = 0;
t_yamamoto 0:669ef71cba68 31 sendMotorData[1].direction8to9.all = 0;
t_yamamoto 0:669ef71cba68 32 #endif
t_yamamoto 0:669ef71cba68 33 #if MOUNTING_MOTOR_NUM > 26
t_yamamoto 0:669ef71cba68 34 sendMotorData[2].direction0to3.all = 0;
t_yamamoto 0:669ef71cba68 35 sendMotorData[2].direction4to7.all = 0;
t_yamamoto 0:669ef71cba68 36 sendMotorData[2].direction8to9.all = 0;
t_yamamoto 0:669ef71cba68 37 #endif
t_yamamoto 0:669ef71cba68 38
t_yamamoto 0:669ef71cba68 39 uint8_t* pwmPo = &sendMotorData[0].pwm0;
t_yamamoto 0:669ef71cba68 40 for(uint8_t i=0; i<MOUNTING_MOTOR_NUM; i++) {
t_yamamoto 0:669ef71cba68 41 *pwmPo = 0;
t_yamamoto 0:669ef71cba68 42 pwmPo++;
t_yamamoto 0:669ef71cba68 43 }
t_yamamoto 0:669ef71cba68 44
t_yamamoto 0:669ef71cba68 45 SetDefault();
t_yamamoto 0:669ef71cba68 46 }
t_yamamoto 0:669ef71cba68 47
t_yamamoto 0:669ef71cba68 48 void Motor::Update(MotorStatus *status) {
t_yamamoto 0:669ef71cba68 49 for(uint8_t i=0; i<MOUNTING_MOTOR_NUM; i++) motor[i] = status[i];
t_yamamoto 0:669ef71cba68 50
t_yamamoto 0:669ef71cba68 51 #if MOUNTING_MOTOR_NUM > 0
t_yamamoto 0:669ef71cba68 52 sendMotorData[0].direction0to3.data0 = motor[0].dir;
t_yamamoto 0:669ef71cba68 53 sendMotorData[0].direction0to3.data1 = motor[1].dir;
t_yamamoto 0:669ef71cba68 54 sendMotorData[0].direction0to3.data2 = motor[2].dir;
t_yamamoto 0:669ef71cba68 55 sendMotorData[0].direction0to3.data3 = motor[3].dir;
t_yamamoto 0:669ef71cba68 56 sendMotorData[0].direction4to7.data0 = motor[4].dir;
t_yamamoto 0:669ef71cba68 57 sendMotorData[0].direction4to7.data1 = motor[5].dir;
t_yamamoto 0:669ef71cba68 58 sendMotorData[0].direction4to7.data2 = motor[6].dir;
t_yamamoto 0:669ef71cba68 59 sendMotorData[0].direction4to7.data3 = motor[7].dir;
t_yamamoto 0:669ef71cba68 60 sendMotorData[0].direction8to9.data0 = motor[8].dir;
t_yamamoto 0:669ef71cba68 61 sendMotorData[0].direction8to9.data1 = motor[9].dir;
t_yamamoto 0:669ef71cba68 62 #endif
t_yamamoto 0:669ef71cba68 63 #if MOUNTING_MOTOR_NUM > 13
t_yamamoto 0:669ef71cba68 64 sendMotorData[1].direction0to3.data0 = motor[13].dir;
t_yamamoto 0:669ef71cba68 65 sendMotorData[1].direction0to3.data1 = motor[14].dir;
t_yamamoto 0:669ef71cba68 66 sendMotorData[1].direction0to3.data2 = motor[15].dir;
t_yamamoto 0:669ef71cba68 67 sendMotorData[1].direction0to3.data3 = motor[16].dir;
t_yamamoto 0:669ef71cba68 68 sendMotorData[1].direction4to7.data0 = motor[17].dir;
t_yamamoto 0:669ef71cba68 69 sendMotorData[1].direction4to7.data1 = motor[18].dir;
t_yamamoto 0:669ef71cba68 70 sendMotorData[1].direction4to7.data2 = motor[19].dir;
t_yamamoto 0:669ef71cba68 71 sendMotorData[1].direction4to7.data3 = motor[20].dir;
t_yamamoto 0:669ef71cba68 72 sendMotorData[1].direction8to9.data0 = motor[21].dir;
t_yamamoto 0:669ef71cba68 73 sendMotorData[1].direction8to9.data1 = motor[22].dir;
t_yamamoto 0:669ef71cba68 74 #endif
t_yamamoto 0:669ef71cba68 75 #if MOUNTING_MOTOR_NUM > 26
t_yamamoto 0:669ef71cba68 76 sendMotorData[2].direction0to3.data0 = motor[26].dir;
t_yamamoto 0:669ef71cba68 77 sendMotorData[2].direction0to3.data1 = motor[27].dir;
t_yamamoto 0:669ef71cba68 78 sendMotorData[2].direction0to3.data2 = motor[28].dir;
t_yamamoto 0:669ef71cba68 79 sendMotorData[2].direction0to3.data3 = motor[29].dir;
t_yamamoto 0:669ef71cba68 80 sendMotorData[2].direction4to7.data0 = motor[30].dir;
t_yamamoto 0:669ef71cba68 81 sendMotorData[2].direction4to7.data1 = motor[31].dir;
t_yamamoto 0:669ef71cba68 82 sendMotorData[2].direction4to7.data2 = motor[32].dir;
t_yamamoto 0:669ef71cba68 83 sendMotorData[2].direction4to7.data3 = motor[33].dir;
t_yamamoto 0:669ef71cba68 84 sendMotorData[2].direction8to9.data0 = motor[34].dir;
t_yamamoto 0:669ef71cba68 85 sendMotorData[2].direction8to9.data1 = motor[35].dir;
t_yamamoto 0:669ef71cba68 86 #endif
t_yamamoto 0:669ef71cba68 87
t_yamamoto 0:669ef71cba68 88 uint8_t* pwmPo = &sendMotorData[0].pwm0;
t_yamamoto 0:669ef71cba68 89 for (uint8_t i = 0;i < MOUNTING_MOTOR_NUM;i++)
t_yamamoto 0:669ef71cba68 90 {
t_yamamoto 0:669ef71cba68 91 *pwmPo = motor[i].pwm;
t_yamamoto 0:669ef71cba68 92 pwmPo++;
t_yamamoto 0:669ef71cba68 93 }
t_yamamoto 0:669ef71cba68 94 }
t_yamamoto 0:669ef71cba68 95
t_yamamoto 0:669ef71cba68 96 void Motor::SetDefault() {
t_yamamoto 0:669ef71cba68 97 for(uint8_t i=0; i<MOUNTING_MOTOR_NUM; i++) {
t_yamamoto 0:669ef71cba68 98 motor[i].dir = FREE;
t_yamamoto 0:669ef71cba68 99 motor[i].pwm = 0;
t_yamamoto 0:669ef71cba68 100 }
t_yamamoto 0:669ef71cba68 101 }
t_yamamoto 0:669ef71cba68 102 }
t_yamamoto 0:669ef71cba68 103
t_yamamoto 0:669ef71cba68 104 namespace SOLENOID {
t_yamamoto 0:669ef71cba68 105 SolenoidStatus sendSolenoidData;
t_yamamoto 0:669ef71cba68 106
t_yamamoto 0:669ef71cba68 107 void Solenoid::Initialize() {
t_yamamoto 0:669ef71cba68 108 sendSolenoidData.all = ALL_SOLENOID_OFF;
t_yamamoto 0:669ef71cba68 109 }
t_yamamoto 0:669ef71cba68 110
t_yamamoto 0:669ef71cba68 111 void Solenoid::Update(SolenoidStatus status) {
t_yamamoto 0:669ef71cba68 112 sendSolenoidData.all = status.all;
t_yamamoto 0:669ef71cba68 113 }
t_yamamoto 0:669ef71cba68 114 }
t_yamamoto 0:669ef71cba68 115
t_yamamoto 0:669ef71cba68 116 void ActuatorHub::Update() {
t_yamamoto 0:669ef71cba68 117 if(!RS485SendBuffer.InAnyData()) {
t_yamamoto 0:669ef71cba68 118 // __disable_irq();
t_yamamoto 0:669ef71cba68 119
t_yamamoto 0:669ef71cba68 120 #ifdef USE_MOTOR
t_yamamoto 0:669ef71cba68 121 #if MOUNTING_MOTOR_NUM > 0
t_yamamoto 0:669ef71cba68 122 RS485SendBuffer.PutData('*');
t_yamamoto 0:669ef71cba68 123 RS485SendBuffer.PutData(MOTOR_ADDR);
t_yamamoto 0:669ef71cba68 124 RS485SendBuffer.PutData(MOTOR::sendMotorData[0].direction0to3.all);
t_yamamoto 0:669ef71cba68 125 RS485SendBuffer.PutData(MOTOR::sendMotorData[0].direction4to7.all);
t_yamamoto 0:669ef71cba68 126 RS485SendBuffer.PutData(MOTOR::sendMotorData[0].direction8to9.all);
t_yamamoto 0:669ef71cba68 127 uint8_t* pwmPo = &MOTOR::sendMotorData[0].pwm0;
t_yamamoto 0:669ef71cba68 128 for(uint8_t i=0; i<13; i++) {
t_yamamoto 0:669ef71cba68 129 RS485SendBuffer.PutData(*pwmPo);
t_yamamoto 0:669ef71cba68 130 pwmPo++;
t_yamamoto 0:669ef71cba68 131 }
t_yamamoto 0:669ef71cba68 132 RS485SendBuffer.PutData('\r');
t_yamamoto 0:669ef71cba68 133 #endif
t_yamamoto 0:669ef71cba68 134
t_yamamoto 0:669ef71cba68 135 #if MOUNTING_MOTOR_NUM > 13
t_yamamoto 0:669ef71cba68 136 RS485SendBuffer.PutData('*');
t_yamamoto 0:669ef71cba68 137 RS485SendBuffer.PutData(MOTOR2_ADDR);
t_yamamoto 0:669ef71cba68 138 RS485SendBuffer.PutData(MOTOR::sendMotorData[1].direction0to3.all);
t_yamamoto 0:669ef71cba68 139 RS485SendBuffer.PutData(MOTOR::sendMotorData[1].direction4to7.all);
t_yamamoto 0:669ef71cba68 140 RS485SendBuffer.PutData(MOTOR::sendMotorData[1].direction8to9.all);
t_yamamoto 0:669ef71cba68 141 pwmPo = &MOTOR::sendMotorData[1].pwm0;
t_yamamoto 0:669ef71cba68 142 for(uint8_t i=0; i<13; i++) {
t_yamamoto 0:669ef71cba68 143 RS485SendBuffer.PutData(*pwmPo);
t_yamamoto 0:669ef71cba68 144 pwmPo++;
t_yamamoto 0:669ef71cba68 145 }
t_yamamoto 0:669ef71cba68 146 RS485SendBuffer.PutData('\r');
t_yamamoto 0:669ef71cba68 147 #endif
t_yamamoto 0:669ef71cba68 148
t_yamamoto 0:669ef71cba68 149 #if MOUNTING_MOTOR_NUM > 26
t_yamamoto 0:669ef71cba68 150 RS485SendBuffer.PutData('*');
t_yamamoto 0:669ef71cba68 151 RS485SendBuffer.PutData(MOTOR3_ADDR);
t_yamamoto 0:669ef71cba68 152 RS485SendBuffer.PutData(MOTOR::sendMotorData[2].direction0to3.all);
t_yamamoto 0:669ef71cba68 153 RS485SendBuffer.PutData(MOTOR::sendMotorData[2].direction4to7.all);
t_yamamoto 0:669ef71cba68 154 RS485SendBuffer.PutData(MOTOR::sendMotorData[2].direction8to9.all);
t_yamamoto 0:669ef71cba68 155 pwmPo = &MOTOR::sendMotorData[2].pwm0;
t_yamamoto 0:669ef71cba68 156 for(uint8_t i=0; i<(MOUNTING_MOTOR_NUM-26); i++) {
t_yamamoto 0:669ef71cba68 157 RS485SendBuffer.PutData(*pwmPo);
t_yamamoto 0:669ef71cba68 158 pwmPo++;
t_yamamoto 0:669ef71cba68 159 }
t_yamamoto 0:669ef71cba68 160 RS485SendBuffer.PutData('\r');
t_yamamoto 0:669ef71cba68 161 #endif
t_yamamoto 0:669ef71cba68 162 #endif
t_yamamoto 0:669ef71cba68 163
t_yamamoto 0:669ef71cba68 164 #ifdef USE_SOLENOID
t_yamamoto 0:669ef71cba68 165 RS485SendBuffer.PutData('*');
t_yamamoto 0:669ef71cba68 166 RS485SendBuffer.PutData(SOLENOID_ADDR);
t_yamamoto 0:669ef71cba68 167 RS485SendBuffer.PutData((SOLENOID::sendSolenoidData.all & 0xff00) >> 8);
t_yamamoto 0:669ef71cba68 168 RS485SendBuffer.PutData(SOLENOID::sendSolenoidData.all & 0x00ff);
t_yamamoto 0:669ef71cba68 169 RS485SendBuffer.PutData('\r');
t_yamamoto 0:669ef71cba68 170 #endif
t_yamamoto 0:669ef71cba68 171
t_yamamoto 0:669ef71cba68 172 #ifdef USE_BLUETOOTH
t_yamamoto 0:669ef71cba68 173 if(BluetoothSendBuffer.InAnyData()) {
t_yamamoto 0:669ef71cba68 174 RS485SendBuffer.PutData('*');
t_yamamoto 0:669ef71cba68 175 RS485SendBuffer.PutData(BLUETOOTH_ADDR);
t_yamamoto 0:669ef71cba68 176 while(BluetoothSendBuffer.InAnyData()) {
t_yamamoto 0:669ef71cba68 177 RS485SendBuffer.PutData(BluetoothSendBuffer.GetData());
t_yamamoto 0:669ef71cba68 178 }
t_yamamoto 0:669ef71cba68 179 RS485SendBuffer.PutData(disconnect);
t_yamamoto 0:669ef71cba68 180 }
t_yamamoto 0:669ef71cba68 181 #endif
kishibekairohan 11:028a150943b5 182
kishibekairohan 11:028a150943b5 183 RS485SendBuffer.PutData('*');
kishibekairohan 11:028a150943b5 184 RS485SendBuffer.PutData(TAPELED_ADDR);
kishibekairohan 11:028a150943b5 185 RS485SendBuffer.PutData(sendLedData.red);
kishibekairohan 11:028a150943b5 186 RS485SendBuffer.PutData(sendLedData.green);
kishibekairohan 11:028a150943b5 187 RS485SendBuffer.PutData(sendLedData.blue);
kishibekairohan 11:028a150943b5 188 RS485SendBuffer.PutData('\r');
t_yamamoto 0:669ef71cba68 189
kishibekairohan 11:028a150943b5 190 __enable_irq();
t_yamamoto 0:669ef71cba68 191 }
t_yamamoto 0:669ef71cba68 192 }
t_yamamoto 0:669ef71cba68 193 }
t_yamamoto 0:669ef71cba68 194