Akihiro Nakabayashi / Mbed 2 deprecated MainBoard2018_Auto_Master_A_new

huhuh

Dependencies:   mbed QEI

Fork of MainBoard2018_Auto_Master_A by Taiki Maruyama

Communication/RS485/ActuatorHub/ActuatorHub.cpp@6:10e22bc327ce, 2018-10-01 (annotated)

Committer:
7ka884
Date:
Mon Oct 01 14:01:03 2018 +0000
Revision:
6:10e22bc327ce
Parent:
0:669ef71cba68 
huhuhu

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