大季 矢花 / Mbed 2 deprecated MB2019_main_alltimes_1123

aa

Dependencies:   mbed

System/Process/Process.cpp@30:8f092276b2ba, 2019-10-22 (annotated)

Committer:
M_souta
Date:
Tue Oct 22 02:03:26 2019 +0000
Revision:
30:8f092276b2ba
Parent:
29:44d5454ce8fa
Child:
32:985ec7913f2a
iii

Who changed what in which revision?

UserRevisionLine numberNew contents of line
t_yamamoto 0:669ef71cba68 1 #include "mbed.h"
t_yamamoto 0:669ef71cba68 2 #include "Process.h"
M_souta 30:8f092276b2ba 3
Ryosei 27:dd9f27fce7d1 4 #include <stdlib.h>
7ka884 4:ba9df71868df 5 #include "../../CommonLibraries/PID/PID.h"
t_yamamoto 0:669ef71cba68 6 #include "../../Communication/RS485/ActuatorHub/ActuatorHub.h"
M_souta 21:e3b58d675c1c 7 #include "../../Communication/RS485/LineHub/LineHub.h"
t_yamamoto 0:669ef71cba68 8 #include "../../Communication/Controller/Controller.h"
M_souta 30:8f092276b2ba 9 #include "../../Switch/Switch.h"
Ryosei 27:dd9f27fce7d1 10
t_yamamoto 0:669ef71cba68 11 #include "../../LED/LED.h"
t_yamamoto 0:669ef71cba68 12 #include "../../Safty/Safty.h"
t_yamamoto 0:669ef71cba68 13 #include "../Using.h"
t_yamamoto 0:669ef71cba68 14
t_yamamoto 0:669ef71cba68 15 using namespace SWITCH;
7ka884 4:ba9df71868df 16 using namespace PID_SPACE;
Ryosei 27:dd9f27fce7d1 17
M_souta 21:e3b58d675c1c 18 using namespace LINEHUB;
t_yamamoto 0:669ef71cba68 19
M_souta 22:7d93f79a3686 20
t_yamamoto 0:669ef71cba68 21 static CONTROLLER::ControllerData *controller;
t_yamamoto 0:669ef71cba68 22 ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
t_yamamoto 0:669ef71cba68 23 ACTUATORHUB::SOLENOID::SolenoidStatus solenoid;
t_yamamoto 0:669ef71cba68 24
t_yamamoto 0:669ef71cba68 25 static bool lock;
t_yamamoto 0:669ef71cba68 26 static bool processChangeComp;
t_yamamoto 0:669ef71cba68 27 static int current;
t_yamamoto 0:669ef71cba68 28
t_yamamoto 0:669ef71cba68 29 static void AllActuatorReset();
t_yamamoto 0:669ef71cba68 30
t_yamamoto 0:669ef71cba68 31 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 32 static void (*Process[USE_PROCESS_NUM])(void);
t_yamamoto 0:669ef71cba68 33 #endif
t_yamamoto 0:669ef71cba68 34
t_yamamoto 0:669ef71cba68 35 #pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto 0:669ef71cba68 36
t_yamamoto 0:669ef71cba68 37 /*Replace here with the definition code of your variables.*/
t_yamamoto 0:669ef71cba68 38
kishibekairohan 2:c015739085d3 39
M_souta 30:8f092276b2ba 40 //Serial pc(USBTX, USBRX);
kishibekairohan 16:3f2c2d89372b 41
Ryosei 27:dd9f27fce7d1 42 const int omni[15][15] = {
Ryosei 27:dd9f27fce7d1 43 { 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255 },
Ryosei 27:dd9f27fce7d1 44 { -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255 },
Ryosei 27:dd9f27fce7d1 45 { -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255 },
Ryosei 27:dd9f27fce7d1 46 { -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255 },
Ryosei 27:dd9f27fce7d1 47 { -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255 },
Ryosei 27:dd9f27fce7d1 48 { -130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255 },
Ryosei 27:dd9f27fce7d1 49 { -187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255 },
Ryosei 27:dd9f27fce7d1 50 { -255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255 },
Ryosei 27:dd9f27fce7d1 51 { -255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187 },
Ryosei 27:dd9f27fce7d1 52 { -255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130 },
Ryosei 27:dd9f27fce7d1 53 { -255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83 },
Ryosei 27:dd9f27fce7d1 54 { -255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47 },
Ryosei 27:dd9f27fce7d1 55 { -255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21 },
Ryosei 27:dd9f27fce7d1 56 { -255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5 },
Ryosei 27:dd9f27fce7d1 57 { -255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0 }
M_souta 21:e3b58d675c1c 58 };
M_souta 21:e3b58d675c1c 59
M_souta 21:e3b58d675c1c 60 const int curve[15] = { -204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204 };
M_souta 21:e3b58d675c1c 61
M_souta 29:44d5454ce8fa 62
M_souta 21:e3b58d675c1c 63 uint8_t SetStatus(int);
M_souta 21:e3b58d675c1c 64 uint8_t SetPWM(int);
M_souta 18:c694bae76e51 65
t_yamamoto 0:669ef71cba68 66 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto 0:669ef71cba68 67
t_yamamoto 0:669ef71cba68 68 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 69 #if USE_PROCESS_NUM>0
t_yamamoto 0:669ef71cba68 70 static void Process0(void);
t_yamamoto 0:669ef71cba68 71 #endif
t_yamamoto 0:669ef71cba68 72 #if USE_PROCESS_NUM>1
t_yamamoto 0:669ef71cba68 73 static void Process1(void);
t_yamamoto 0:669ef71cba68 74 #endif
t_yamamoto 0:669ef71cba68 75 #if USE_PROCESS_NUM>2
t_yamamoto 0:669ef71cba68 76 static void Process2(void);
t_yamamoto 0:669ef71cba68 77 #endif
t_yamamoto 0:669ef71cba68 78 #if USE_PROCESS_NUM>3
t_yamamoto 0:669ef71cba68 79 static void Process3(void);
t_yamamoto 0:669ef71cba68 80 #endif
t_yamamoto 0:669ef71cba68 81 #if USE_PROCESS_NUM>4
t_yamamoto 0:669ef71cba68 82 static void Process4(void);
t_yamamoto 0:669ef71cba68 83 #endif
t_yamamoto 0:669ef71cba68 84 #if USE_PROCESS_NUM>5
t_yamamoto 0:669ef71cba68 85 static void Process5(void);
t_yamamoto 0:669ef71cba68 86 #endif
t_yamamoto 0:669ef71cba68 87 #if USE_PROCESS_NUM>6
t_yamamoto 0:669ef71cba68 88 static void Process6(void);
t_yamamoto 0:669ef71cba68 89 #endif
t_yamamoto 0:669ef71cba68 90 #if USE_PROCESS_NUM>7
t_yamamoto 0:669ef71cba68 91 static void Process7(void);
t_yamamoto 0:669ef71cba68 92 #endif
t_yamamoto 0:669ef71cba68 93 #if USE_PROCESS_NUM>8
t_yamamoto 0:669ef71cba68 94 static void Process8(void);
t_yamamoto 0:669ef71cba68 95 #endif
t_yamamoto 0:669ef71cba68 96 #if USE_PROCESS_NUM>9
t_yamamoto 0:669ef71cba68 97 static void Process9(void);
t_yamamoto 0:669ef71cba68 98 #endif
t_yamamoto 0:669ef71cba68 99 #endif
t_yamamoto 0:669ef71cba68 100
t_yamamoto 0:669ef71cba68 101 void SystemProcessInitialize()
t_yamamoto 0:669ef71cba68 102 {
Ryosei 27:dd9f27fce7d1 103 #pragma region USER-DEFINED_VARIABLE_INIT
Ryosei 27:dd9f27fce7d1 104 /*Replace here with the initialization code of your variables.*/
Ryosei 27:dd9f27fce7d1 105
Ryosei 27:dd9f27fce7d1 106 #pragma endregion USER-DEFINED_VARIABLE_INIT
Ryosei 27:dd9f27fce7d1 107
Ryosei 27:dd9f27fce7d1 108 lock = true;
Ryosei 27:dd9f27fce7d1 109 processChangeComp = true;
Ryosei 27:dd9f27fce7d1 110 current = DEFAULT_PROCESS;
t_yamamoto 0:669ef71cba68 111
Ryosei 27:dd9f27fce7d1 112 #ifdef USE_SUBPROCESS
Ryosei 27:dd9f27fce7d1 113 #if USE_PROCESS_NUM>0
Ryosei 27:dd9f27fce7d1 114 Process[0] = Process0;
Ryosei 27:dd9f27fce7d1 115 #endif
Ryosei 27:dd9f27fce7d1 116 #if USE_PROCESS_NUM>1
Ryosei 27:dd9f27fce7d1 117 Process[1] = Process1;
Ryosei 27:dd9f27fce7d1 118 #endif
Ryosei 27:dd9f27fce7d1 119 #if USE_PROCESS_NUM>2
Ryosei 27:dd9f27fce7d1 120 Process[2] = Process2;
Ryosei 27:dd9f27fce7d1 121 #endif
Ryosei 27:dd9f27fce7d1 122 #if USE_PROCESS_NUM>3
Ryosei 27:dd9f27fce7d1 123 Process[3] = Process3;
Ryosei 27:dd9f27fce7d1 124 #endif
Ryosei 27:dd9f27fce7d1 125 #if USE_PROCESS_NUM>4
Ryosei 27:dd9f27fce7d1 126 Process[4] = Process4;
Ryosei 27:dd9f27fce7d1 127 #endif
Ryosei 27:dd9f27fce7d1 128 #if USE_PROCESS_NUM>5
Ryosei 27:dd9f27fce7d1 129 Process[5] = Process5;
Ryosei 27:dd9f27fce7d1 130 #endif
Ryosei 27:dd9f27fce7d1 131 #if USE_PROCESS_NUM>6
Ryosei 27:dd9f27fce7d1 132 Process[6] = Process6;
Ryosei 27:dd9f27fce7d1 133 #endif
Ryosei 27:dd9f27fce7d1 134 #if USE_PROCESS_NUM>7
Ryosei 27:dd9f27fce7d1 135 Process[7] = Process7;
Ryosei 27:dd9f27fce7d1 136 #endif
Ryosei 27:dd9f27fce7d1 137 #if USE_PROCESS_NUM>8
Ryosei 27:dd9f27fce7d1 138 Process[8] = Process8;
Ryosei 27:dd9f27fce7d1 139 #endif
Ryosei 27:dd9f27fce7d1 140 #if USE_PROCESS_NUM>9
Ryosei 27:dd9f27fce7d1 141 Process[9] = Process9;
Ryosei 27:dd9f27fce7d1 142 #endif
Ryosei 27:dd9f27fce7d1 143 #endif
t_yamamoto 0:669ef71cba68 144 }
t_yamamoto 0:669ef71cba68 145
t_yamamoto 0:669ef71cba68 146 static void SystemProcessUpdate()
t_yamamoto 0:669ef71cba68 147 {
Ryosei 27:dd9f27fce7d1 148 #ifdef USE_SUBPROCESS
Ryosei 27:dd9f27fce7d1 149 if(controller->Button.HOME) lock = false;
Ryosei 27:dd9f27fce7d1 150
Ryosei 27:dd9f27fce7d1 151 if(controller->Button.START && processChangeComp) {
Ryosei 27:dd9f27fce7d1 152 current++;
Ryosei 27:dd9f27fce7d1 153 if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
Ryosei 27:dd9f27fce7d1 154 processChangeComp = false;
Ryosei 27:dd9f27fce7d1 155 } else if(controller->Button.SELECT && processChangeComp) {
Ryosei 27:dd9f27fce7d1 156 current--;
Ryosei 27:dd9f27fce7d1 157 if (current < 0) current = 0;
Ryosei 27:dd9f27fce7d1 158 processChangeComp = false;
Ryosei 27:dd9f27fce7d1 159 } else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
Ryosei 27:dd9f27fce7d1 160 #endif
Ryosei 27:dd9f27fce7d1 161
Ryosei 27:dd9f27fce7d1 162 #ifdef USE_MOTOR
Ryosei 27:dd9f27fce7d1 163 ACTUATORHUB::MOTOR::Motor::Update(motor);
Ryosei 27:dd9f27fce7d1 164 #endif
Ryosei 27:dd9f27fce7d1 165
Ryosei 27:dd9f27fce7d1 166 #ifdef USE_SOLENOID
Ryosei 27:dd9f27fce7d1 167 ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
Ryosei 27:dd9f27fce7d1 168 #endif
Ryosei 27:dd9f27fce7d1 169
Ryosei 27:dd9f27fce7d1 170 #ifdef USE_RS485
Ryosei 27:dd9f27fce7d1 171 ACTUATORHUB::ActuatorHub::Update();
Ryosei 27:dd9f27fce7d1 172 //LINEHUB::LineHub::Update();
Ryosei 27:dd9f27fce7d1 173 #endif
Ryosei 27:dd9f27fce7d1 174
Ryosei 27:dd9f27fce7d1 175 }
Ryosei 27:dd9f27fce7d1 176
Ryosei 27:dd9f27fce7d1 177 void SystemProcess()
Ryosei 27:dd9f27fce7d1 178 {
Ryosei 27:dd9f27fce7d1 179 SystemProcessInitialize();
M_souta 30:8f092276b2ba 180
Ryosei 27:dd9f27fce7d1 181 while(1) {
Ryosei 27:dd9f27fce7d1 182
M_souta 30:8f092276b2ba 183 #ifdef USE_MU
Ryosei 27:dd9f27fce7d1 184 controller = CONTROLLER::Controller::GetData();
M_souta 30:8f092276b2ba 185 #endif
Ryosei 27:dd9f27fce7d1 186
M_souta 30:8f092276b2ba 187 #ifdef USE_ERRORCHECK
M_souta 29:44d5454ce8fa 188 if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost) {
Ryosei 27:dd9f27fce7d1 189 CONTROLLER::Controller::DataReset();
Ryosei 27:dd9f27fce7d1 190 AllActuatorReset();
Ryosei 27:dd9f27fce7d1 191 lock = true;
Ryosei 27:dd9f27fce7d1 192 } else
M_souta 30:8f092276b2ba 193 #endif
Ryosei 27:dd9f27fce7d1 194 {
Ryosei 27:dd9f27fce7d1 195
M_souta 30:8f092276b2ba 196 #ifdef USE_SUBPROCESS
Ryosei 27:dd9f27fce7d1 197 if(!lock) {
Ryosei 27:dd9f27fce7d1 198 Process[current]();
Ryosei 27:dd9f27fce7d1 199 } else
M_souta 30:8f092276b2ba 200 #endif
Ryosei 27:dd9f27fce7d1 201 {
Ryosei 27:dd9f27fce7d1 202 //ロック時の処理
Ryosei 27:dd9f27fce7d1 203 }
Ryosei 27:dd9f27fce7d1 204 }
M_souta 30:8f092276b2ba 205
Ryosei 27:dd9f27fce7d1 206 SystemProcessUpdate();
Ryosei 27:dd9f27fce7d1 207 }
t_yamamoto 0:669ef71cba68 208 }
t_yamamoto 0:669ef71cba68 209
kishibekairohan 2:c015739085d3 210
kishibekairohan 2:c015739085d3 211
kishibekairohan 2:c015739085d3 212
t_yamamoto 0:669ef71cba68 213 #pragma region PROCESS
t_yamamoto 0:669ef71cba68 214 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 215 #if USE_PROCESS_NUM>0
Ryosei 27:dd9f27fce7d1 216 static void Process0()
Ryosei 27:dd9f27fce7d1 217 {
Ryosei 27:dd9f27fce7d1 218 AllActuatorReset();
t_yamamoto 0:669ef71cba68 219 }
t_yamamoto 0:669ef71cba68 220 #endif
t_yamamoto 0:669ef71cba68 221
t_yamamoto 0:669ef71cba68 222 #if USE_PROCESS_NUM>1
kishibekairohan 16:3f2c2d89372b 223 static void Process1()
t_yamamoto 0:669ef71cba68 224 {
Ryosei 27:dd9f27fce7d1 225
t_yamamoto 0:669ef71cba68 226 }
t_yamamoto 0:669ef71cba68 227 #endif
t_yamamoto 0:669ef71cba68 228
t_yamamoto 0:669ef71cba68 229 #if USE_PROCESS_NUM>2
kishibekairohan 16:3f2c2d89372b 230 static void Process2()
Ryosei 27:dd9f27fce7d1 231 {
M_souta 26:4c0ce2f05688 232
kishibekairohan 13:b6e02d6261d7 233 }
kishibekairohan 13:b6e02d6261d7 234 #endif
kishibekairohan 13:b6e02d6261d7 235
kishibekairohan 13:b6e02d6261d7 236 #if USE_PROCESS_NUM>3
Ryosei 27:dd9f27fce7d1 237 static void Process3()
kishibekairohan 13:b6e02d6261d7 238 {
M_souta 29:44d5454ce8fa 239
kishibekairohan 13:b6e02d6261d7 240 }
kishibekairohan 13:b6e02d6261d7 241 #endif
kishibekairohan 13:b6e02d6261d7 242
kishibekairohan 13:b6e02d6261d7 243 #if USE_PROCESS_NUM>4
Ryosei 27:dd9f27fce7d1 244 static void Process4()
M_souta 22:7d93f79a3686 245 {
M_souta 29:44d5454ce8fa 246
kishibekairohan 13:b6e02d6261d7 247 }
kishibekairohan 13:b6e02d6261d7 248 #endif
kishibekairohan 13:b6e02d6261d7 249
kishibekairohan 13:b6e02d6261d7 250 #if USE_PROCESS_NUM>5
Ryosei 27:dd9f27fce7d1 251 static void Process5()
Ryosei 27:dd9f27fce7d1 252 {
Ryosei 27:dd9f27fce7d1 253
kishibekairohan 13:b6e02d6261d7 254 }
kishibekairohan 13:b6e02d6261d7 255 #endif
kishibekairohan 13:b6e02d6261d7 256
kishibekairohan 13:b6e02d6261d7 257 #if USE_PROCESS_NUM>6
Ryosei 27:dd9f27fce7d1 258 static void Process6()
kishibekairohan 13:b6e02d6261d7 259 {
Ryosei 27:dd9f27fce7d1 260
kishibekairohan 13:b6e02d6261d7 261 }
kishibekairohan 13:b6e02d6261d7 262 #endif
kishibekairohan 13:b6e02d6261d7 263
kishibekairohan 13:b6e02d6261d7 264 #if USE_PROCESS_NUM>7
kishibekairohan 13:b6e02d6261d7 265 static void Process7()
kishibekairohan 13:b6e02d6261d7 266 {
M_souta 29:44d5454ce8fa 267
kishibekairohan 12:c09b3e08a316 268 }
kishibekairohan 12:c09b3e08a316 269 #endif
kishibekairohan 12:c09b3e08a316 270
Ryosei 27:dd9f27fce7d1 271 #if USE_PROCESS_NUM>8
kishibekairohan 13:b6e02d6261d7 272 static void Process8()
kishibekairohan 12:c09b3e08a316 273 {
M_souta 29:44d5454ce8fa 274
kishibekairohan 12:c09b3e08a316 275 }
kishibekairohan 12:c09b3e08a316 276 #endif
kishibekairohan 12:c09b3e08a316 277
kishibekairohan 13:b6e02d6261d7 278 #if USE_PROCESS_NUM>9
kishibekairohan 13:b6e02d6261d7 279 static void Process9()
kishibekairohan 12:c09b3e08a316 280 {
Ryosei 27:dd9f27fce7d1 281
t_yamamoto 0:669ef71cba68 282 }
t_yamamoto 0:669ef71cba68 283 #endif
t_yamamoto 0:669ef71cba68 284 #endif
t_yamamoto 0:669ef71cba68 285 #pragma endregion PROCESS
t_yamamoto 0:669ef71cba68 286
t_yamamoto 0:669ef71cba68 287 static void AllActuatorReset()
t_yamamoto 0:669ef71cba68 288 {
M_souta 29:44d5454ce8fa 289 #ifdef USE_SOLENOID
M_souta 29:44d5454ce8fa 290 solenoid.all = ALL_SOLENOID_OFF;
M_souta 29:44d5454ce8fa 291 #endif
M_souta 29:44d5454ce8fa 292
M_souta 29:44d5454ce8fa 293 #ifdef USE_MOTOR
M_souta 29:44d5454ce8fa 294 for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) {
M_souta 29:44d5454ce8fa 295 motor[i].dir = FREE;
M_souta 29:44d5454ce8fa 296 motor[i].pwm = 0;
M_souta 29:44d5454ce8fa 297 }
M_souta 29:44d5454ce8fa 298 #endif
t_yamamoto 0:669ef71cba68 299 }
t_yamamoto 0:669ef71cba68 300
kishibekairohan 16:3f2c2d89372b 301 #pragma region USER-DEFINED-FUNCTIONS
M_souta 29:44d5454ce8fa 302
M_souta 29:44d5454ce8fa 303 uint8_t SetStatus(int pwmVal)
Ryosei 27:dd9f27fce7d1 304 {
M_souta 29:44d5454ce8fa 305 if (pwmVal < 0) return BACK;
M_souta 29:44d5454ce8fa 306 else if (pwmVal > 0) return FOR;
M_souta 29:44d5454ce8fa 307 else if (pwmVal == 0) return BRAKE;
M_souta 29:44d5454ce8fa 308 else return BRAKE;
M_souta 22:7d93f79a3686 309 }
kishibekairohan 16:3f2c2d89372b 310
M_souta 29:44d5454ce8fa 311 uint8_t SetPWM(int pwmVal)
Ryosei 27:dd9f27fce7d1 312 {
M_souta 29:44d5454ce8fa 313 if (pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255;
M_souta 29:44d5454ce8fa 314 else return abs(pwmVal);
M_souta 22:7d93f79a3686 315 }
kishibekairohan 16:3f2c2d89372b 316
t_yamamoto 0:669ef71cba68 317 #pragma endregion