大季 矢花 / Mbed 2 deprecated MB2019_main_11_17_hiru

aa

Dependencies:   mbed

System/Process/Process.cpp@32:985ec7913f2a, 2019-10-22 (annotated)

Committer:
M_souta
Date:
Tue Oct 22 02:50:54 2019 +0000
Revision:
32:985ec7913f2a
Parent:
30:8f092276b2ba
Child:
33:dd598bb23ad1
iyfyfif

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