Miya Miyagawa / Mbed 2 deprecated Main_2019_B_Auto_TAMAYA

The last version programs

Dependencies:   mbed TrapezoidControl Pulse QEI

System/Process/Process.cpp@24:370616a56815, 2019-09-18 (annotated)

Committer:
M_souta
Date:
Wed Sep 18 02:03:56 2019 +0000
Revision:
24:370616a56815
Parent:
23:c853372cf626
Child:
25:b3a9f34b201d
ii

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kishibekairohan 13:b6e02d6261d7 1
t_yamamoto 0:669ef71cba68 2 #include "mbed.h"
t_yamamoto 0:669ef71cba68 3 #include "Process.h"
t_yamamoto 0:669ef71cba68 4
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"
t_yamamoto 0:669ef71cba68 9 #include "../../Input/ExternalInt/ExternalInt.h"
t_yamamoto 0:669ef71cba68 10 #include "../../Input/Switch/Switch.h"
kishibekairohan 16:3f2c2d89372b 11 #include "../../Input/Encoder/Encoder.h"
M_souta 22:7d93f79a3686 12 #include "../../Input/Ultrasonic/Ultrasonic.h"
t_yamamoto 0:669ef71cba68 13 #include "../../LED/LED.h"
t_yamamoto 0:669ef71cba68 14 #include "../../Safty/Safty.h"
t_yamamoto 0:669ef71cba68 15 #include "../Using.h"
t_yamamoto 0:669ef71cba68 16
t_yamamoto 0:669ef71cba68 17 using namespace SWITCH;
7ka884 4:ba9df71868df 18 using namespace PID_SPACE;
kishibekairohan 16:3f2c2d89372b 19 using namespace ENCODER;
M_souta 22:7d93f79a3686 20 using namespace ULTRASONIC;
M_souta 21:e3b58d675c1c 21 using namespace LINEHUB;
t_yamamoto 0:669ef71cba68 22
M_souta 22:7d93f79a3686 23
t_yamamoto 0:669ef71cba68 24 static CONTROLLER::ControllerData *controller;
t_yamamoto 0:669ef71cba68 25 ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
t_yamamoto 0:669ef71cba68 26 ACTUATORHUB::SOLENOID::SolenoidStatus solenoid;
t_yamamoto 0:669ef71cba68 27
t_yamamoto 0:669ef71cba68 28 static bool lock;
t_yamamoto 0:669ef71cba68 29 static bool processChangeComp;
t_yamamoto 0:669ef71cba68 30 static int current;
t_yamamoto 0:669ef71cba68 31
t_yamamoto 0:669ef71cba68 32 static void AllActuatorReset();
t_yamamoto 0:669ef71cba68 33
t_yamamoto 0:669ef71cba68 34 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 35 static void (*Process[USE_PROCESS_NUM])(void);
t_yamamoto 0:669ef71cba68 36 #endif
t_yamamoto 0:669ef71cba68 37
t_yamamoto 0:669ef71cba68 38 #pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto 0:669ef71cba68 39
t_yamamoto 0:669ef71cba68 40 /*Replace here with the definition code of your variables.*/
t_yamamoto 0:669ef71cba68 41
M_souta 22:7d93f79a3686 42 USS ultrasonic[] = {
M_souta 22:7d93f79a3686 43 USS(ECHO_0,TRIG_0,TEMP),
M_souta 22:7d93f79a3686 44 USS(ECHO_1,TRIG_1,TEMP),
M_souta 22:7d93f79a3686 45 };
kishibekairohan 2:c015739085d3 46
M_souta 22:7d93f79a3686 47 Serial pc(USBTX, USBRX);
kishibekairohan 7:e88c5d47a3be 48
kishibekairohan 16:3f2c2d89372b 49 //**************Buzzer****************
kishibekairohan 11:028a150943b5 50 //DigitalOut buzzer(BUZZER_PIN);
kishibekairohan 9:f93fc79a49ea 51 void BuzzerTimer_func();
kishibekairohan 9:f93fc79a49ea 52 Ticker BuzzerTimer;
kishibekairohan 16:3f2c2d89372b 53 bool EMGflag = false;
kishibekairohan 16:3f2c2d89372b 54 PwmOut buzzer(BUZZER_PIN);
kishibekairohan 16:3f2c2d89372b 55 //**************Buzzer****************
kishibekairohan 11:028a150943b5 56
kishibekairohan 11:028a150943b5 57 //************TapeLed*****************
kishibekairohan 11:028a150943b5 58 void TapeLedEms_func();
kishibekairohan 11:028a150943b5 59 TapeLedData tapeLED;
kishibekairohan 11:028a150943b5 60 TapeLedData sendLedData;
kishibekairohan 11:028a150943b5 61 TapeLED_Mode ledMode = Normal;
kishibekairohan 11:028a150943b5 62 Ticker tapeLedTimer;
kishibekairohan 11:028a150943b5 63 //************TapaLed*****************
kishibekairohan 16:3f2c2d89372b 64
M_souta 22:7d93f79a3686 65 //*************** lift ***************
M_souta 22:7d93f79a3686 66 #define LOWER 1
M_souta 22:7d93f79a3686 67 #define MIDDLRE 2
M_souta 22:7d93f79a3686 68 #define UPPER 3
M_souta 22:7d93f79a3686 69 uint8_t liftState = LOWER;
M_souta 22:7d93f79a3686 70 bool moving = false;
M_souta 22:7d93f79a3686 71 bool switchFlag_LB = false;
M_souta 22:7d93f79a3686 72 bool switchFlag_RB = false;
M_souta 22:7d93f79a3686 73
M_souta 22:7d93f79a3686 74 //*************** lift ***************
M_souta 22:7d93f79a3686 75
M_souta 22:7d93f79a3686 76 //*************tire*************
M_souta 22:7d93f79a3686 77 PID rotaconPID[] = {
M_souta 22:7d93f79a3686 78 PID(0.0001,-1,1,0.05,0,0), //LF
M_souta 22:7d93f79a3686 79 PID(0.0001,-1,1,0.05,0,0), //LB
M_souta 22:7d93f79a3686 80 PID(0.0001,-1,1,0.05,0,0), //RB
M_souta 22:7d93f79a3686 81 PID(0.0001,-1,1,0.05,0,0), //RF
M_souta 22:7d93f79a3686 82 };
M_souta 22:7d93f79a3686 83
M_souta 22:7d93f79a3686 84 #define FL 0
M_souta 22:7d93f79a3686 85 #define BL 1
M_souta 22:7d93f79a3686 86 #define BR 2
M_souta 22:7d93f79a3686 87 #define FR 3
M_souta 22:7d93f79a3686 88
M_souta 22:7d93f79a3686 89 #define PI 3.141592
M_souta 22:7d93f79a3686 90
M_souta 22:7d93f79a3686 91 const float tireR = 101.6; //タイヤの半径
M_souta 22:7d93f79a3686 92 const float ucR = 420.0; //中心からのタイヤの距離
M_souta 22:7d93f79a3686 93
M_souta 22:7d93f79a3686 94 typedef struct {
M_souta 22:7d93f79a3686 95 float Vx; //X方向の速度
M_souta 22:7d93f79a3686 96 float Vy; //Y方向の速度
M_souta 22:7d93f79a3686 97 float Va; //角速度
M_souta 22:7d93f79a3686 98 } Vvector;
M_souta 22:7d93f79a3686 99
M_souta 22:7d93f79a3686 100 Vvector move; //進む速度
M_souta 22:7d93f79a3686 101 Vvector correction_LT; //ライントレースの補正速度
M_souta 22:7d93f79a3686 102 Vvector synthetic; //合成速度
M_souta 22:7d93f79a3686 103
M_souta 22:7d93f79a3686 104 float sita = 0;
M_souta 22:7d93f79a3686 105
M_souta 22:7d93f79a3686 106 bool PIDflag = false;
M_souta 22:7d93f79a3686 107
M_souta 22:7d93f79a3686 108 int linePara[8];
M_souta 22:7d93f79a3686 109 int linePara_U;
M_souta 22:7d93f79a3686 110 int linePara_B;
M_souta 22:7d93f79a3686 111 int linePara_L;
M_souta 22:7d93f79a3686 112 int linePara_R;
M_souta 22:7d93f79a3686 113
M_souta 22:7d93f79a3686 114 #define FL 0
M_souta 22:7d93f79a3686 115 #define BL 1
M_souta 22:7d93f79a3686 116 #define BR 2
M_souta 22:7d93f79a3686 117 #define FR 3
M_souta 22:7d93f79a3686 118
M_souta 22:7d93f79a3686 119 float tireProcessRPM[4];
M_souta 22:7d93f79a3686 120 float tireTargetMaxRPM[4];
M_souta 22:7d93f79a3686 121 float tireTargetRPM[4];
M_souta 22:7d93f79a3686 122
M_souta 22:7d93f79a3686 123 float tirePWM[4];
M_souta 22:7d93f79a3686 124
M_souta 22:7d93f79a3686 125 float timePV[4];
M_souta 22:7d93f79a3686 126 float timeCV[4];
M_souta 22:7d93f79a3686 127 float pulsePV[4];
M_souta 22:7d93f79a3686 128 float pulseCV[4];
M_souta 22:7d93f79a3686 129
M_souta 22:7d93f79a3686 130 void tirePID();
M_souta 22:7d93f79a3686 131 int lineCast(char k);
M_souta 22:7d93f79a3686 132
M_souta 22:7d93f79a3686 133 Timer rotaconSampling;
M_souta 22:7d93f79a3686 134 Ticker rotaconPIDtimer;
M_souta 22:7d93f79a3686 135
M_souta 22:7d93f79a3686 136 bool countFlag;
M_souta 22:7d93f79a3686 137 //*************tire**************//
M_souta 22:7d93f79a3686 138
M_souta 22:7d93f79a3686 139 // ************* Line ************** //
M_souta 22:7d93f79a3686 140
M_souta 22:7d93f79a3686 141 float pw = 0;
M_souta 22:7d93f79a3686 142 int lineFase = 0;
M_souta 22:7d93f79a3686 143 bool lineCheck = false;
M_souta 22:7d93f79a3686 144 int linePWM;
M_souta 22:7d93f79a3686 145 int adj_F;
M_souta 22:7d93f79a3686 146 int adj_B;
M_souta 22:7d93f79a3686 147
M_souta 22:7d93f79a3686 148 int mode = 0;
M_souta 22:7d93f79a3686 149
M_souta 22:7d93f79a3686 150 int lineCount = 0;
M_souta 22:7d93f79a3686 151
M_souta 22:7d93f79a3686 152 // ************* Line ************** //
M_souta 22:7d93f79a3686 153
M_souta 21:e3b58d675c1c 154 const int omni[15][15] =
M_souta 20:eae8c84f318c 155 {
M_souta 21:e3b58d675c1c 156 { 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255 },
M_souta 21:e3b58d675c1c 157 { -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255 },
M_souta 21:e3b58d675c1c 158 { -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255 },
M_souta 21:e3b58d675c1c 159 { -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255 },
M_souta 21:e3b58d675c1c 160 { -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255 },
M_souta 21:e3b58d675c1c 161 { -130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255 },
M_souta 21:e3b58d675c1c 162 { -187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255 },
M_souta 21:e3b58d675c1c 163 { -255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255 },
M_souta 21:e3b58d675c1c 164 { -255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187 },
M_souta 21:e3b58d675c1c 165 { -255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130 },
M_souta 21:e3b58d675c1c 166 { -255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83 },
M_souta 21:e3b58d675c1c 167 { -255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47 },
M_souta 21:e3b58d675c1c 168 { -255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21 },
M_souta 21:e3b58d675c1c 169 { -255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5 },
M_souta 21:e3b58d675c1c 170 { -255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0 }
M_souta 21:e3b58d675c1c 171 };
M_souta 21:e3b58d675c1c 172
M_souta 21:e3b58d675c1c 173 const int curve[15] = { -204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204 };
M_souta 21:e3b58d675c1c 174
M_souta 21:e3b58d675c1c 175 uint8_t SetStatus(int);
M_souta 21:e3b58d675c1c 176 uint8_t SetStatus(int pwmVal) {
M_souta 21:e3b58d675c1c 177 if (pwmVal < 0) return BACK;
M_souta 21:e3b58d675c1c 178 else if (pwmVal > 0) return FOR;
M_souta 21:e3b58d675c1c 179 else if (pwmVal == 0) return BRAKE;
M_souta 21:e3b58d675c1c 180 else return BRAKE;
M_souta 20:eae8c84f318c 181 }
M_souta 21:e3b58d675c1c 182 uint8_t SetPWM(int);
M_souta 21:e3b58d675c1c 183 uint8_t SetPWM(int pwmVal) {
M_souta 21:e3b58d675c1c 184 if (pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255;
M_souta 21:e3b58d675c1c 185 else return abs(pwmVal);
M_souta 21:e3b58d675c1c 186 }
M_souta 18:c694bae76e51 187
t_yamamoto 0:669ef71cba68 188 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto 0:669ef71cba68 189
t_yamamoto 0:669ef71cba68 190 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 191 #if USE_PROCESS_NUM>0
t_yamamoto 0:669ef71cba68 192 static void Process0(void);
t_yamamoto 0:669ef71cba68 193 #endif
t_yamamoto 0:669ef71cba68 194 #if USE_PROCESS_NUM>1
t_yamamoto 0:669ef71cba68 195 static void Process1(void);
t_yamamoto 0:669ef71cba68 196 #endif
t_yamamoto 0:669ef71cba68 197 #if USE_PROCESS_NUM>2
t_yamamoto 0:669ef71cba68 198 static void Process2(void);
t_yamamoto 0:669ef71cba68 199 #endif
t_yamamoto 0:669ef71cba68 200 #if USE_PROCESS_NUM>3
t_yamamoto 0:669ef71cba68 201 static void Process3(void);
t_yamamoto 0:669ef71cba68 202 #endif
t_yamamoto 0:669ef71cba68 203 #if USE_PROCESS_NUM>4
t_yamamoto 0:669ef71cba68 204 static void Process4(void);
t_yamamoto 0:669ef71cba68 205 #endif
t_yamamoto 0:669ef71cba68 206 #if USE_PROCESS_NUM>5
t_yamamoto 0:669ef71cba68 207 static void Process5(void);
t_yamamoto 0:669ef71cba68 208 #endif
t_yamamoto 0:669ef71cba68 209 #if USE_PROCESS_NUM>6
t_yamamoto 0:669ef71cba68 210 static void Process6(void);
t_yamamoto 0:669ef71cba68 211 #endif
t_yamamoto 0:669ef71cba68 212 #if USE_PROCESS_NUM>7
t_yamamoto 0:669ef71cba68 213 static void Process7(void);
t_yamamoto 0:669ef71cba68 214 #endif
t_yamamoto 0:669ef71cba68 215 #if USE_PROCESS_NUM>8
t_yamamoto 0:669ef71cba68 216 static void Process8(void);
t_yamamoto 0:669ef71cba68 217 #endif
t_yamamoto 0:669ef71cba68 218 #if USE_PROCESS_NUM>9
t_yamamoto 0:669ef71cba68 219 static void Process9(void);
t_yamamoto 0:669ef71cba68 220 #endif
t_yamamoto 0:669ef71cba68 221 #endif
t_yamamoto 0:669ef71cba68 222
t_yamamoto 0:669ef71cba68 223 void SystemProcessInitialize()
t_yamamoto 0:669ef71cba68 224 {
t_yamamoto 0:669ef71cba68 225 #pragma region USER-DEFINED_VARIABLE_INIT
kishibekairohan 16:3f2c2d89372b 226 /*Replace here with the initialization code of your variables.*/
M_souta 22:7d93f79a3686 227 rotaconPIDtimer.attach(tirePID,0.1);
M_souta 21:e3b58d675c1c 228
t_yamamoto 0:669ef71cba68 229 #pragma endregion USER-DEFINED_VARIABLE_INIT
t_yamamoto 0:669ef71cba68 230
t_yamamoto 0:669ef71cba68 231 lock = true;
t_yamamoto 0:669ef71cba68 232 processChangeComp = true;
t_yamamoto 0:669ef71cba68 233 current = DEFAULT_PROCESS;
t_yamamoto 0:669ef71cba68 234
t_yamamoto 0:669ef71cba68 235 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 236 #if USE_PROCESS_NUM>0
t_yamamoto 0:669ef71cba68 237 Process[0] = Process0;
t_yamamoto 0:669ef71cba68 238 #endif
t_yamamoto 0:669ef71cba68 239 #if USE_PROCESS_NUM>1
t_yamamoto 0:669ef71cba68 240 Process[1] = Process1;
t_yamamoto 0:669ef71cba68 241 #endif
t_yamamoto 0:669ef71cba68 242 #if USE_PROCESS_NUM>2
t_yamamoto 0:669ef71cba68 243 Process[2] = Process2;
t_yamamoto 0:669ef71cba68 244 #endif
t_yamamoto 0:669ef71cba68 245 #if USE_PROCESS_NUM>3
t_yamamoto 0:669ef71cba68 246 Process[3] = Process3;
t_yamamoto 0:669ef71cba68 247 #endif
t_yamamoto 0:669ef71cba68 248 #if USE_PROCESS_NUM>4
t_yamamoto 0:669ef71cba68 249 Process[4] = Process4;
t_yamamoto 0:669ef71cba68 250 #endif
t_yamamoto 0:669ef71cba68 251 #if USE_PROCESS_NUM>5
t_yamamoto 0:669ef71cba68 252 Process[5] = Process5;
t_yamamoto 0:669ef71cba68 253 #endif
t_yamamoto 0:669ef71cba68 254 #if USE_PROCESS_NUM>6
t_yamamoto 0:669ef71cba68 255 Process[6] = Process6;
t_yamamoto 0:669ef71cba68 256 #endif
t_yamamoto 0:669ef71cba68 257 #if USE_PROCESS_NUM>7
t_yamamoto 0:669ef71cba68 258 Process[7] = Process7;
t_yamamoto 0:669ef71cba68 259 #endif
t_yamamoto 0:669ef71cba68 260 #if USE_PROCESS_NUM>8
t_yamamoto 0:669ef71cba68 261 Process[8] = Process8;
t_yamamoto 0:669ef71cba68 262 #endif
t_yamamoto 0:669ef71cba68 263 #if USE_PROCESS_NUM>9
t_yamamoto 0:669ef71cba68 264 Process[9] = Process9;
t_yamamoto 0:669ef71cba68 265 #endif
t_yamamoto 0:669ef71cba68 266 #endif
t_yamamoto 0:669ef71cba68 267 }
t_yamamoto 0:669ef71cba68 268
t_yamamoto 0:669ef71cba68 269 static void SystemProcessUpdate()
t_yamamoto 0:669ef71cba68 270 {
t_yamamoto 0:669ef71cba68 271 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 272 if(controller->Button.HOME) lock = false;
t_yamamoto 0:669ef71cba68 273
t_yamamoto 0:669ef71cba68 274 if(controller->Button.START && processChangeComp)
t_yamamoto 0:669ef71cba68 275 {
t_yamamoto 0:669ef71cba68 276 current++;
t_yamamoto 0:669ef71cba68 277 if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
t_yamamoto 0:669ef71cba68 278 processChangeComp = false;
t_yamamoto 0:669ef71cba68 279 }
t_yamamoto 0:669ef71cba68 280 else if(controller->Button.SELECT && processChangeComp)
t_yamamoto 0:669ef71cba68 281 {
t_yamamoto 0:669ef71cba68 282 current--;
t_yamamoto 0:669ef71cba68 283 if (current < 0) current = 0;
t_yamamoto 0:669ef71cba68 284 processChangeComp = false;
t_yamamoto 0:669ef71cba68 285 }
t_yamamoto 0:669ef71cba68 286 else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
t_yamamoto 0:669ef71cba68 287 #endif
t_yamamoto 0:669ef71cba68 288
t_yamamoto 0:669ef71cba68 289 #ifdef USE_MOTOR
t_yamamoto 0:669ef71cba68 290 ACTUATORHUB::MOTOR::Motor::Update(motor);
t_yamamoto 0:669ef71cba68 291 #endif
t_yamamoto 0:669ef71cba68 292
t_yamamoto 0:669ef71cba68 293 #ifdef USE_SOLENOID
t_yamamoto 0:669ef71cba68 294 ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
t_yamamoto 0:669ef71cba68 295 #endif
t_yamamoto 0:669ef71cba68 296
t_yamamoto 0:669ef71cba68 297 #ifdef USE_RS485
t_yamamoto 0:669ef71cba68 298 ACTUATORHUB::ActuatorHub::Update();
M_souta 21:e3b58d675c1c 299 //LINEHUB::LineHub::Update();
t_yamamoto 0:669ef71cba68 300 #endif
t_yamamoto 0:669ef71cba68 301
t_yamamoto 0:669ef71cba68 302 }
t_yamamoto 0:669ef71cba68 303
kishibekairohan 2:c015739085d3 304
kishibekairohan 2:c015739085d3 305
t_yamamoto 0:669ef71cba68 306 void SystemProcess()
t_yamamoto 0:669ef71cba68 307 {
t_yamamoto 0:669ef71cba68 308 SystemProcessInitialize();
t_yamamoto 0:669ef71cba68 309
t_yamamoto 0:669ef71cba68 310 while(1)
kishibekairohan 15:dfcec98f5aa9 311 {
M_souta 21:e3b58d675c1c 312 int g[8];
M_souta 21:e3b58d675c1c 313 for(int i = 0; i < 8; i++){
M_souta 22:7d93f79a3686 314 g[i] = lineCast(LineHub::GetPara(i));
M_souta 21:e3b58d675c1c 315 }
M_souta 22:7d93f79a3686 316
M_souta 22:7d93f79a3686 317 pc.printf("1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 7:%d 8:%d\n\r",g[0],g[1],g[2],g[3],g[4],g[5],g[6],g[7]);
M_souta 22:7d93f79a3686 318
M_souta 22:7d93f79a3686 319 //float a = ultrasonic[0].ReadDis();
M_souta 22:7d93f79a3686 320 //pc.printf("%f\n\r",a);
M_souta 22:7d93f79a3686 321
M_souta 22:7d93f79a3686 322 //int ppap = encoder[0].getPulses();
M_souta 22:7d93f79a3686 323 //pc.printf("%d\n\r",ppap);
M_souta 22:7d93f79a3686 324
M_souta 20:eae8c84f318c 325
kishibekairohan 17:50dc4b449e69 326 buzzer.period(1.0/800);
kishibekairohan 15:dfcec98f5aa9 327
t_yamamoto 0:669ef71cba68 328 #ifdef USE_MU
t_yamamoto 0:669ef71cba68 329 controller = CONTROLLER::Controller::GetData();
t_yamamoto 0:669ef71cba68 330 #endif
t_yamamoto 0:669ef71cba68 331
t_yamamoto 0:669ef71cba68 332 #ifdef USE_ERRORCHECK
t_yamamoto 0:669ef71cba68 333 if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost)
t_yamamoto 0:669ef71cba68 334 {
t_yamamoto 0:669ef71cba68 335 CONTROLLER::Controller::DataReset();
t_yamamoto 0:669ef71cba68 336 AllActuatorReset();
t_yamamoto 0:669ef71cba68 337 lock = true;
t_yamamoto 0:669ef71cba68 338 }
t_yamamoto 0:669ef71cba68 339 else
t_yamamoto 0:669ef71cba68 340 #endif
t_yamamoto 0:669ef71cba68 341 {
t_yamamoto 0:669ef71cba68 342
t_yamamoto 0:669ef71cba68 343 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 344 if(!lock)
t_yamamoto 0:669ef71cba68 345 {
t_yamamoto 0:669ef71cba68 346 Process[current]();
t_yamamoto 0:669ef71cba68 347 }
t_yamamoto 0:669ef71cba68 348 else
t_yamamoto 0:669ef71cba68 349 #endif
t_yamamoto 0:669ef71cba68 350 {
t_yamamoto 0:669ef71cba68 351 //ロック時の処理
t_yamamoto 0:669ef71cba68 352 }
t_yamamoto 0:669ef71cba68 353 }
t_yamamoto 0:669ef71cba68 354
M_souta 22:7d93f79a3686 355
kishibekairohan 16:3f2c2d89372b 356 //Emergency!
M_souta 22:7d93f79a3686 357 /*
kishibekairohan 17:50dc4b449e69 358 if(!EMG_0 && !EMG_1 && !EMGflag){
kishibekairohan 17:50dc4b449e69 359 buzzer = 0;
kishibekairohan 17:50dc4b449e69 360 BuzzerTimer.attach(BuzzerTimer_func, 1);
kishibekairohan 17:50dc4b449e69 361 EMGflag = true;
kishibekairohan 17:50dc4b449e69 362 LED_DEBUG0 = 1;
kishibekairohan 9:f93fc79a49ea 363 }
kishibekairohan 17:50dc4b449e69 364 if(EMG_0 && EMG_1 && EMGflag){
kishibekairohan 17:50dc4b449e69 365 buzzer = 1;
kishibekairohan 9:f93fc79a49ea 366 BuzzerTimer.detach();
kishibekairohan 17:50dc4b449e69 367 EMGflag = false;
kishibekairohan 9:f93fc79a49ea 368 }
M_souta 22:7d93f79a3686 369 */
t_yamamoto 0:669ef71cba68 370 SystemProcessUpdate();
t_yamamoto 0:669ef71cba68 371 }
t_yamamoto 0:669ef71cba68 372 }
t_yamamoto 0:669ef71cba68 373
kishibekairohan 2:c015739085d3 374
kishibekairohan 2:c015739085d3 375
kishibekairohan 2:c015739085d3 376
t_yamamoto 0:669ef71cba68 377 #pragma region PROCESS
t_yamamoto 0:669ef71cba68 378 #ifdef USE_SUBPROCESS
t_yamamoto 0:669ef71cba68 379 #if USE_PROCESS_NUM>0
kishibekairohan 12:c09b3e08a316 380 static void Process0()
kishibekairohan 10:1295d39fec3a 381 {
M_souta 21:e3b58d675c1c 382 AllActuatorReset();
t_yamamoto 0:669ef71cba68 383 }
t_yamamoto 0:669ef71cba68 384 #endif
t_yamamoto 0:669ef71cba68 385
t_yamamoto 0:669ef71cba68 386 #if USE_PROCESS_NUM>1
kishibekairohan 16:3f2c2d89372b 387 static void Process1()
t_yamamoto 0:669ef71cba68 388 {
M_souta 22:7d93f79a3686 389
M_souta 22:7d93f79a3686 390 PIDflag = false;
M_souta 22:7d93f79a3686 391
M_souta 21:e3b58d675c1c 392 if(controller->Button.UP) {
M_souta 21:e3b58d675c1c 393 motor[LIFT_LB].dir = FOR;
M_souta 23:c853372cf626 394 motor[LIFT_LB].pwm = 190;
M_souta 21:e3b58d675c1c 395 motor[LIFT_RB].dir = BACK;
M_souta 21:e3b58d675c1c 396 motor[LIFT_RB].pwm = 180;
M_souta 21:e3b58d675c1c 397 } else if(controller->Button.DOWN) {
M_souta 21:e3b58d675c1c 398 motor[LIFT_LB].dir = BACK;
M_souta 21:e3b58d675c1c 399 motor[LIFT_LB].pwm = 180;
M_souta 21:e3b58d675c1c 400 motor[LIFT_RB].dir = FOR;
M_souta 23:c853372cf626 401 motor[LIFT_RB].pwm = 190;
M_souta 23:c853372cf626 402 } else if(controller->Button.LEFT) {
M_souta 23:c853372cf626 403 motor[LIFT_LB].dir = FOR;
M_souta 23:c853372cf626 404 motor[LIFT_LB].pwm = 180;
M_souta 23:c853372cf626 405 } else if(controller->Button.RIGHT) {
M_souta 23:c853372cf626 406 motor[LIFT_RB].dir = BACK;
M_souta 21:e3b58d675c1c 407 motor[LIFT_RB].pwm = 180;
M_souta 21:e3b58d675c1c 408 } else {
M_souta 23:c853372cf626 409 motor[LIFT_LB].dir = FREE;
M_souta 23:c853372cf626 410 motor[LIFT_LB].pwm = 255;
M_souta 23:c853372cf626 411 motor[LIFT_RB].dir = BACK;
M_souta 23:c853372cf626 412 motor[LIFT_RB].pwm = 10;
M_souta 21:e3b58d675c1c 413 }
M_souta 22:7d93f79a3686 414
7ka884 4:ba9df71868df 415
M_souta 21:e3b58d675c1c 416 if(controller->Button.X) {
M_souta 21:e3b58d675c1c 417 motor[LIFT_U].dir = FOR;
M_souta 21:e3b58d675c1c 418 motor[LIFT_U].pwm = 180;
M_souta 21:e3b58d675c1c 419 } else if(controller->Button.Y) {
M_souta 21:e3b58d675c1c 420 motor[LIFT_U].dir = BACK;
M_souta 21:e3b58d675c1c 421 motor[LIFT_U].pwm = 180;
M_souta 21:e3b58d675c1c 422 } else {
M_souta 21:e3b58d675c1c 423 motor[LIFT_U].dir = BRAKE;
M_souta 21:e3b58d675c1c 424 motor[LIFT_U].pwm = 180;
M_souta 21:e3b58d675c1c 425 }
M_souta 21:e3b58d675c1c 426
M_souta 21:e3b58d675c1c 427 if(!(controller->AnalogL.Y == 7) || !(controller->AnalogL.X == 7)) {
M_souta 21:e3b58d675c1c 428 motor[TIRE_BL].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] );
M_souta 21:e3b58d675c1c 429 motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X] );
M_souta 21:e3b58d675c1c 430 motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] );
M_souta 21:e3b58d675c1c 431 motor[TIRE_FR].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y] );
M_souta 21:e3b58d675c1c 432
M_souta 22:7d93f79a3686 433 motor[TIRE_FR].pwm = SetPWM(omni[controller->AnalogL.Y][14-controller->AnalogL.X] * 0.2) ;
M_souta 22:7d93f79a3686 434 motor[TIRE_FL].pwm = SetPWM(omni[controller->AnalogL.Y][controller->AnalogL.X] * 0.2) ;
M_souta 22:7d93f79a3686 435 motor[TIRE_BR].pwm = SetPWM(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] * 0.2) ;
M_souta 22:7d93f79a3686 436 motor[TIRE_BL].pwm = SetPWM(omni[controller->AnalogL.X][14-controller->AnalogL.Y] * 0.2) ;
M_souta 21:e3b58d675c1c 437 } else {
M_souta 21:e3b58d675c1c 438 motor[TIRE_BL].dir = SetStatus(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 439 motor[TIRE_FL].dir = SetStatus(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 440 motor[TIRE_BR].dir = SetStatus(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 441 motor[TIRE_FR].dir = SetStatus(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 442
M_souta 21:e3b58d675c1c 443 motor[TIRE_FR].pwm = SetPWM(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 444 motor[TIRE_FL].pwm = SetPWM(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 445 motor[TIRE_BR].pwm = SetPWM(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 446 motor[TIRE_BL].pwm = SetPWM(curve[controller->AnalogR.X]);
M_souta 21:e3b58d675c1c 447 }
t_yamamoto 0:669ef71cba68 448 }
t_yamamoto 0:669ef71cba68 449 #endif
t_yamamoto 0:669ef71cba68 450
t_yamamoto 0:669ef71cba68 451 #if USE_PROCESS_NUM>2
kishibekairohan 16:3f2c2d89372b 452 static void Process2()
kishibekairohan 13:b6e02d6261d7 453 {
M_souta 22:7d93f79a3686 454 /*
M_souta 22:7d93f79a3686 455 if(moving) {
M_souta 22:7d93f79a3686 456 if(LimiSw::IsPressed(LSW_LB)) {
M_souta 22:7d93f79a3686 457 if(switchFlag_LB) {
M_souta 22:7d93f79a3686 458 switchFlag_LB = false;
M_souta 22:7d93f79a3686 459 motor[LIFT_LB].dir = BRAKE;
M_souta 22:7d93f79a3686 460 motor[LIFT_LB].pwm = 200;
M_souta 22:7d93f79a3686 461 } else {
M_souta 22:7d93f79a3686 462 seitchFlag_LB = true;
M_souta 22:7d93f79a3686 463 }
M_souta 22:7d93f79a3686 464 }
M_souta 22:7d93f79a3686 465 if(LimiSw::IsPressed(LSW_RB)) {
M_souta 22:7d93f79a3686 466 if(switchFlag_RB) {
M_souta 22:7d93f79a3686 467 switchFlag_RB = false;
M_souta 22:7d93f79a3686 468 motor[LIFT_RB].dir = BRAKE;
M_souta 22:7d93f79a3686 469 motor[LIFT_RB].pwm = 200;
M_souta 22:7d93f79a3686 470 } else {
M_souta 22:7d93f79a3686 471 seitchFlag_RB = true;
M_souta 22:7d93f79a3686 472 }
M_souta 22:7d93f79a3686 473 }
M_souta 22:7d93f79a3686 474 if(motor[LIFT_LB].dir == BRAKE && motor[LIFT_RB].dir == BRAKE) moving = false;
M_souta 22:7d93f79a3686 475 } else {
M_souta 22:7d93f79a3686 476 if(controller->Button.UP) {
M_souta 22:7d93f79a3686 477 if(!(state == UPPER)) {
M_souta 22:7d93f79a3686 478 state++;
M_souta 22:7d93f79a3686 479 motor[LIFT_LB].dir = BACK;
M_souta 22:7d93f79a3686 480 motor[LIFT_RB].dir = FOR;
M_souta 22:7d93f79a3686 481 motor[LIFT_LB].pwm = 200;
M_souta 22:7d93f79a3686 482 motor[LIFT_RB].pwm = 200;
M_souta 22:7d93f79a3686 483 }
M_souta 22:7d93f79a3686 484 } else if(controller->Button.DOWN) {
M_souta 22:7d93f79a3686 485 if(!(state == LOWER)) {
M_souta 22:7d93f79a3686 486 state--;
M_souta 22:7d93f79a3686 487 moving = true;
M_souta 22:7d93f79a3686 488 motor[LIFT_LB].dir = FOR;
M_souta 22:7d93f79a3686 489 motor[LIFT_RB].dir = BACK;
M_souta 22:7d93f79a3686 490 motor[LIFT_LB].pwm = 200;
M_souta 22:7d93f79a3686 491 motor[LIFT_RB].pwm = 200;
M_souta 22:7d93f79a3686 492 }
M_souta 22:7d93f79a3686 493 } else {
M_souta 22:7d93f79a3686 494 motor[LIFT_LB].dir = BRAKE;
M_souta 22:7d93f79a3686 495 motor[LIFT_RB].dir = BRAKE;
M_souta 22:7d93f79a3686 496 motor[LIFT_LB].pwm = 200;
M_souta 22:7d93f79a3686 497 motor[LIFT_RB].pwm = 200;
M_souta 22:7d93f79a3686 498 }
M_souta 22:7d93f79a3686 499 }
M_souta 22:7d93f79a3686 500 */
kishibekairohan 13:b6e02d6261d7 501 }
kishibekairohan 13:b6e02d6261d7 502 #endif
kishibekairohan 13:b6e02d6261d7 503
kishibekairohan 13:b6e02d6261d7 504 #if USE_PROCESS_NUM>3
kishibekairohan 16:3f2c2d89372b 505 static void Process3()
kishibekairohan 13:b6e02d6261d7 506 {
M_souta 22:7d93f79a3686 507 AllActuatorReset();
M_souta 23:c853372cf626 508 lineFase = 0;
kishibekairohan 13:b6e02d6261d7 509 }
kishibekairohan 13:b6e02d6261d7 510 #endif
kishibekairohan 13:b6e02d6261d7 511
kishibekairohan 13:b6e02d6261d7 512 #if USE_PROCESS_NUM>4
kishibekairohan 13:b6e02d6261d7 513 static void Process4()
M_souta 22:7d93f79a3686 514 {
M_souta 22:7d93f79a3686 515
M_souta 22:7d93f79a3686 516 static int x,y;
M_souta 22:7d93f79a3686 517 static int count = 0;
M_souta 22:7d93f79a3686 518
M_souta 22:7d93f79a3686 519 linePara_U = lineCast(LineHub::GetPara(0));
M_souta 22:7d93f79a3686 520 linePara_B = lineCast(LineHub::GetPara(2));
M_souta 22:7d93f79a3686 521 linePara_L = lineCast(LineHub::GetPara(3));
M_souta 22:7d93f79a3686 522 linePara_R = lineCast(LineHub::GetPara(4));
M_souta 22:7d93f79a3686 523
M_souta 23:c853372cf626 524 if(linePara_B == 'A' && count == 0) {
M_souta 22:7d93f79a3686 525 lineFase = 1;
M_souta 22:7d93f79a3686 526 }
kishibekairohan 15:dfcec98f5aa9 527
M_souta 22:7d93f79a3686 528 if(lineFase == 0) {
M_souta 23:c853372cf626 529 pw = 0.5;
M_souta 22:7d93f79a3686 530 switch(linePara_U) {
M_souta 22:7d93f79a3686 531 case -2:
M_souta 22:7d93f79a3686 532 x = 5;
M_souta 22:7d93f79a3686 533 y = 3;
M_souta 22:7d93f79a3686 534 break;
M_souta 22:7d93f79a3686 535 case -3:
M_souta 22:7d93f79a3686 536 x = 5;
M_souta 22:7d93f79a3686 537 y = 3;
M_souta 22:7d93f79a3686 538 break;
M_souta 22:7d93f79a3686 539 case -1:
M_souta 22:7d93f79a3686 540 x = 6;
M_souta 22:7d93f79a3686 541 y = 3;
M_souta 22:7d93f79a3686 542 break;
M_souta 22:7d93f79a3686 543 case 0:
M_souta 22:7d93f79a3686 544 x = 7;
M_souta 22:7d93f79a3686 545 y = 3;
M_souta 22:7d93f79a3686 546 break;
M_souta 22:7d93f79a3686 547 case 1:
M_souta 22:7d93f79a3686 548 x = 8;
M_souta 22:7d93f79a3686 549 y = 3;
M_souta 22:7d93f79a3686 550 break;
M_souta 22:7d93f79a3686 551 case 3:
M_souta 22:7d93f79a3686 552 x = 9;
M_souta 22:7d93f79a3686 553 y = 3;
M_souta 22:7d93f79a3686 554 break;
M_souta 22:7d93f79a3686 555 case 2:
M_souta 22:7d93f79a3686 556 x = 9;
M_souta 22:7d93f79a3686 557 y = 3;
M_souta 22:7d93f79a3686 558 break;
M_souta 22:7d93f79a3686 559 case 'A':
M_souta 22:7d93f79a3686 560 lineCheck = true;
M_souta 22:7d93f79a3686 561 x = x;
M_souta 22:7d93f79a3686 562 y = y;
M_souta 22:7d93f79a3686 563 break;
M_souta 22:7d93f79a3686 564 case 'N':
M_souta 22:7d93f79a3686 565 x = 7;
M_souta 22:7d93f79a3686 566 y = 7;
M_souta 22:7d93f79a3686 567 break;
M_souta 22:7d93f79a3686 568 x = 7;
M_souta 22:7d93f79a3686 569 y = 7;
M_souta 22:7d93f79a3686 570 default:
M_souta 22:7d93f79a3686 571 x = 9;
M_souta 22:7d93f79a3686 572 y = 9;
M_souta 22:7d93f79a3686 573 }
M_souta 22:7d93f79a3686 574 if(lineCheck == true && (!(linePara_U) == 'A')) {
M_souta 22:7d93f79a3686 575 count++;
M_souta 22:7d93f79a3686 576 }
M_souta 22:7d93f79a3686 577 } else if(lineFase == 1) {
M_souta 23:c853372cf626 578 pw = 0.3;
M_souta 23:c853372cf626 579 x = 7;
M_souta 23:c853372cf626 580 y = 9;
M_souta 23:c853372cf626 581 if(linePara_R == 0) {
M_souta 22:7d93f79a3686 582 lineFase = 2;
M_souta 22:7d93f79a3686 583 x = 7;
M_souta 22:7d93f79a3686 584 y = 7;
M_souta 22:7d93f79a3686 585 }
M_souta 22:7d93f79a3686 586 } else if(lineFase == 2) {
M_souta 22:7d93f79a3686 587 x = 7;
M_souta 22:7d93f79a3686 588 y = 7;
M_souta 22:7d93f79a3686 589 } else {
M_souta 22:7d93f79a3686 590 x = 7;
M_souta 22:7d93f79a3686 591 y = 7;
M_souta 22:7d93f79a3686 592 }
M_souta 22:7d93f79a3686 593
M_souta 22:7d93f79a3686 594 int t = 0;
M_souta 22:7d93f79a3686 595 if((linePara_U + linePara_B) > 3) t = 1;
M_souta 22:7d93f79a3686 596
M_souta 22:7d93f79a3686 597 if(controller->Button.A) {
M_souta 22:7d93f79a3686 598 motor[TIRE_BL].dir = SetStatus(-omni[y][14-x] );
M_souta 22:7d93f79a3686 599 motor[TIRE_FL].dir = SetStatus(omni[y][x]);
M_souta 22:7d93f79a3686 600 motor[TIRE_BR].dir = SetStatus(-omni[14-x][14-y]);
M_souta 22:7d93f79a3686 601 motor[TIRE_FR].dir = SetStatus(omni[x][14-y]);
M_souta 22:7d93f79a3686 602
M_souta 22:7d93f79a3686 603 motor[TIRE_FR].pwm = SetPWM((omni[y][14-x])) * pw;
M_souta 22:7d93f79a3686 604 motor[TIRE_FL].pwm = SetPWM((omni[y][x])) * pw;
M_souta 22:7d93f79a3686 605 motor[TIRE_BR].pwm = SetPWM((omni[14-x][14-y])) * pw;
M_souta 22:7d93f79a3686 606 motor[TIRE_BL].pwm = SetPWM((omni[x][14-y])) * pw;
M_souta 22:7d93f79a3686 607 } else {
M_souta 22:7d93f79a3686 608 motor[TIRE_BL].dir = SetStatus(0);
M_souta 22:7d93f79a3686 609 motor[TIRE_FL].dir = SetStatus(0);
M_souta 22:7d93f79a3686 610 motor[TIRE_BR].dir = SetStatus(0);
M_souta 22:7d93f79a3686 611 motor[TIRE_FR].dir = SetStatus(0);
M_souta 22:7d93f79a3686 612
M_souta 22:7d93f79a3686 613 motor[TIRE_FR].pwm = SetPWM(0);
M_souta 22:7d93f79a3686 614 motor[TIRE_FL].pwm = SetPWM(0);
M_souta 22:7d93f79a3686 615 motor[TIRE_BR].pwm = SetPWM(0);
M_souta 22:7d93f79a3686 616 motor[TIRE_BL].pwm = SetPWM(0);
M_souta 22:7d93f79a3686 617 }
M_souta 22:7d93f79a3686 618
kishibekairohan 13:b6e02d6261d7 619 }
kishibekairohan 13:b6e02d6261d7 620 #endif
kishibekairohan 13:b6e02d6261d7 621
kishibekairohan 13:b6e02d6261d7 622 #if USE_PROCESS_NUM>5
kishibekairohan 16:3f2c2d89372b 623 static void Process5()
kishibekairohan 16:3f2c2d89372b 624 {
M_souta 22:7d93f79a3686 625 lineFase = 0;
M_souta 22:7d93f79a3686 626 lineCheck = true;
kishibekairohan 13:b6e02d6261d7 627 }
kishibekairohan 13:b6e02d6261d7 628 #endif
kishibekairohan 13:b6e02d6261d7 629
kishibekairohan 13:b6e02d6261d7 630 #if USE_PROCESS_NUM>6
kishibekairohan 13:b6e02d6261d7 631 static void Process6()
kishibekairohan 13:b6e02d6261d7 632 {
kishibekairohan 13:b6e02d6261d7 633
M_souta 22:7d93f79a3686 634 for(int i = 0; i < 8; i++){
M_souta 22:7d93f79a3686 635 linePara[i] = lineCast(LineHub::GetPara(i));
M_souta 22:7d93f79a3686 636 }
M_souta 22:7d93f79a3686 637
M_souta 23:c853372cf626 638 static int count = 100000;
M_souta 22:7d93f79a3686 639 count++;
M_souta 22:7d93f79a3686 640
M_souta 22:7d93f79a3686 641 if(count < 10000) {
M_souta 22:7d93f79a3686 642 lineCheck = false;
M_souta 22:7d93f79a3686 643 } else {
M_souta 22:7d93f79a3686 644 lineCheck = true;
M_souta 22:7d93f79a3686 645 }
M_souta 22:7d93f79a3686 646
M_souta 22:7d93f79a3686 647 if(lineFase == 0) { // 前進
M_souta 22:7d93f79a3686 648 switch(linePara[0]) {
M_souta 22:7d93f79a3686 649 motor[TIRE_FL].dir = FOR;
M_souta 22:7d93f79a3686 650 motor[TIRE_BL].dir = BRAKE;
M_souta 22:7d93f79a3686 651 motor[TIRE_BR].dir = BACK;
M_souta 22:7d93f79a3686 652 motor[TIRE_FR].dir = BRAKE;
M_souta 22:7d93f79a3686 653 motor[TIRE_FL].pwm = 30;
M_souta 22:7d93f79a3686 654 motor[TIRE_FR].pwm = 0;
M_souta 22:7d93f79a3686 655 motor[TIRE_BR].pwm = 30;
M_souta 22:7d93f79a3686 656 motor[TIRE_BL].pwm = 0;
M_souta 22:7d93f79a3686 657 break;
M_souta 22:7d93f79a3686 658 case -3:
M_souta 22:7d93f79a3686 659 motor[TIRE_FL].dir = FOR;
M_souta 23:c853372cf626 660 motor[TIRE_BL].dir = BACK;
M_souta 22:7d93f79a3686 661 motor[TIRE_BR].dir = BACK;
M_souta 23:c853372cf626 662 motor[TIRE_FR].dir = FOR;
M_souta 22:7d93f79a3686 663 motor[TIRE_FL].pwm = 30;
M_souta 22:7d93f79a3686 664 motor[TIRE_FR].pwm = 10;
M_souta 22:7d93f79a3686 665 motor[TIRE_BR].pwm = 30;
M_souta 22:7d93f79a3686 666 motor[TIRE_BL].pwm = 10;
M_souta 22:7d93f79a3686 667 break;
M_souta 22:7d93f79a3686 668 case -1:
M_souta 22:7d93f79a3686 669 motor[TIRE_FL].dir = FOR;
M_souta 23:c853372cf626 670 motor[TIRE_BL].dir = BACK;
M_souta 22:7d93f79a3686 671 motor[TIRE_BR].dir = BACK;
M_souta 23:c853372cf626 672 motor[TIRE_FR].dir = FOR;
M_souta 22:7d93f79a3686 673 motor[TIRE_FL].pwm = 30;
M_souta 22:7d93f79a3686 674 motor[TIRE_FR].pwm = 20;
M_souta 22:7d93f79a3686 675 motor[TIRE_BR].pwm = 30;
M_souta 22:7d93f79a3686 676 motor[TIRE_BL].pwm = 20;
M_souta 22:7d93f79a3686 677 break;
M_souta 22:7d93f79a3686 678 case 0:
M_souta 22:7d93f79a3686 679 motor[TIRE_FL].dir = FOR;
M_souta 23:c853372cf626 680 motor[TIRE_BL].dir = BACK;
M_souta 22:7d93f79a3686 681 motor[TIRE_BR].dir = BACK;
M_souta 23:c853372cf626 682 motor[TIRE_FR].dir = FOR;
M_souta 22:7d93f79a3686 683 motor[TIRE_FL].pwm = 30;
M_souta 22:7d93f79a3686 684 motor[TIRE_FR].pwm = 30;
M_souta 22:7d93f79a3686 685 motor[TIRE_BR].pwm = 30;
M_souta 22:7d93f79a3686 686 motor[TIRE_BL].pwm = 30;
M_souta 22:7d93f79a3686 687 break;
M_souta 22:7d93f79a3686 688 case 1:
M_souta 22:7d93f79a3686 689 motor[TIRE_FL].dir = FOR;
M_souta 23:c853372cf626 690 motor[TIRE_BL].dir = BACK;
M_souta 22:7d93f79a3686 691 motor[TIRE_BR].dir = BACK;
M_souta 23:c853372cf626 692 motor[TIRE_FR].dir = FOR;
M_souta 22:7d93f79a3686 693 motor[TIRE_FL].pwm = 20;
M_souta 22:7d93f79a3686 694 motor[TIRE_FR].pwm = 30;
M_souta 22:7d93f79a3686 695 motor[TIRE_BR].pwm = 20;
M_souta 22:7d93f79a3686 696 motor[TIRE_BL].pwm = 30;
M_souta 22:7d93f79a3686 697 break;
M_souta 22:7d93f79a3686 698 case 3:
M_souta 22:7d93f79a3686 699 motor[TIRE_FL].dir = FOR;
M_souta 22:7d93f79a3686 700 motor[TIRE_BL].dir = FOR;
M_souta 22:7d93f79a3686 701 motor[TIRE_BR].dir = BACK;
M_souta 22:7d93f79a3686 702 motor[TIRE_FR].dir = BACK;
M_souta 22:7d93f79a3686 703 motor[TIRE_FL].pwm = 10;
M_souta 22:7d93f79a3686 704 motor[TIRE_FR].pwm = 30;
M_souta 22:7d93f79a3686 705 motor[TIRE_BR].pwm = 10;
M_souta 22:7d93f79a3686 706 motor[TIRE_BL].pwm = 30;
M_souta 22:7d93f79a3686 707 break;
M_souta 22:7d93f79a3686 708 case 2:
M_souta 22:7d93f79a3686 709 motor[TIRE_FL].dir = BRAKE;
M_souta 22:7d93f79a3686 710 motor[TIRE_BL].dir = FOR;
M_souta 22:7d93f79a3686 711 motor[TIRE_BR].dir = BRAKE;
M_souta 22:7d93f79a3686 712 motor[TIRE_FR].dir = BACK;
M_souta 22:7d93f79a3686 713 motor[TIRE_FL].pwm = 0;
M_souta 22:7d93f79a3686 714 motor[TIRE_FR].pwm = 30;
M_souta 22:7d93f79a3686 715 motor[TIRE_BR].pwm = 0;
M_souta 22:7d93f79a3686 716 motor[TIRE_BL].pwm = 30;
M_souta 22:7d93f79a3686 717 break;
M_souta 22:7d93f79a3686 718 case 'A':
M_souta 22:7d93f79a3686 719 motor[TIRE_FL].dir = FOR;
M_souta 23:c853372cf626 720 motor[TIRE_BL].dir = BACK;
M_souta 22:7d93f79a3686 721 motor[TIRE_BR].dir = BACK;
M_souta 23:c853372cf626 722 motor[TIRE_FR].dir = FOR;
M_souta 22:7d93f79a3686 723 motor[TIRE_FL].pwm = 30;
M_souta 22:7d93f79a3686 724 motor[TIRE_FR].pwm = 30;
M_souta 22:7d93f79a3686 725 motor[TIRE_BR].pwm = 30;
M_souta 22:7d93f79a3686 726 motor[TIRE_BL].pwm = 30;
M_souta 22:7d93f79a3686 727 if(lineCheck == true) {
M_souta 22:7d93f79a3686 728 lineCount++;
M_souta 22:7d93f79a3686 729 count = 0;
M_souta 22:7d93f79a3686 730 }
M_souta 22:7d93f79a3686 731 default:
M_souta 22:7d93f79a3686 732 motor[TIRE_FL].dir = BRAKE;
M_souta 22:7d93f79a3686 733 motor[TIRE_BL].dir = BRAKE;
M_souta 22:7d93f79a3686 734 motor[TIRE_BR].dir = BRAKE;
M_souta 22:7d93f79a3686 735 motor[TIRE_FR].dir = BRAKE;
M_souta 22:7d93f79a3686 736 motor[TIRE_FL].pwm = 30;
M_souta 22:7d93f79a3686 737 motor[TIRE_FR].pwm = 30;
M_souta 22:7d93f79a3686 738 motor[TIRE_BR].pwm = 30;
M_souta 22:7d93f79a3686 739 motor[TIRE_BL].pwm = 30;
M_souta 22:7d93f79a3686 740 }
M_souta 22:7d93f79a3686 741 if(lineCount == 1) {
M_souta 22:7d93f79a3686 742 lineFase = 1;
M_souta 22:7d93f79a3686 743 }
M_souta 22:7d93f79a3686 744 } else if(lineFase == 1) { // 前進 低速
M_souta 22:7d93f79a3686 745 motor[TIRE_FL].dir = FOR;
M_souta 22:7d93f79a3686 746 motor[TIRE_BL].dir = FOR;
M_souta 22:7d93f79a3686 747 motor[TIRE_BR].dir = BACK;
M_souta 22:7d93f79a3686 748 motor[TIRE_FR].dir = BACK;
M_souta 22:7d93f79a3686 749 motor[TIRE_FL].pwm = 15;
M_souta 22:7d93f79a3686 750 motor[TIRE_FR].pwm = 15;
M_souta 22:7d93f79a3686 751 motor[TIRE_BR].pwm = 15;
M_souta 22:7d93f79a3686 752 motor[TIRE_BL].pwm = 15;
M_souta 22:7d93f79a3686 753 if(linePara[4] == 0) {
M_souta 22:7d93f79a3686 754 lineFase = 2;
M_souta 22:7d93f79a3686 755 motor[TIRE_FL].dir = BRAKE;
M_souta 22:7d93f79a3686 756 motor[TIRE_BL].dir = BRAKE;
M_souta 22:7d93f79a3686 757 motor[TIRE_BR].dir = BRAKE;
M_souta 22:7d93f79a3686 758 motor[TIRE_FR].dir = BRAKE;
M_souta 22:7d93f79a3686 759 motor[TIRE_FL].pwm = 30;
M_souta 22:7d93f79a3686 760 motor[TIRE_FR].pwm = 30;
M_souta 22:7d93f79a3686 761 motor[TIRE_BR].pwm = 30;
M_souta 22:7d93f79a3686 762 motor[TIRE_BL].pwm = 30;
M_souta 22:7d93f79a3686 763 }
M_souta 22:7d93f79a3686 764 } else if(lineFase == 2){ // 位置調整
M_souta 22:7d93f79a3686 765 lineFase = 3;
M_souta 22:7d93f79a3686 766 } else if(lineFase == 3){ // 右 直進
M_souta 22:7d93f79a3686 767 motor[TIRE_FL].dir = BRAKE;
M_souta 22:7d93f79a3686 768 motor[TIRE_BL].dir = BRAKE;
M_souta 22:7d93f79a3686 769 motor[TIRE_BR].dir = BRAKE;
M_souta 22:7d93f79a3686 770 motor[TIRE_FR].dir = BRAKE;
M_souta 22:7d93f79a3686 771 motor[TIRE_FL].pwm = 30;
M_souta 22:7d93f79a3686 772 motor[TIRE_FR].pwm = 30;
M_souta 22:7d93f79a3686 773 motor[TIRE_BR].pwm = 30;
M_souta 22:7d93f79a3686 774 motor[TIRE_BL].pwm = 30;
M_souta 22:7d93f79a3686 775 }
M_souta 22:7d93f79a3686 776
kishibekairohan 13:b6e02d6261d7 777 }
kishibekairohan 13:b6e02d6261d7 778 #endif
kishibekairohan 13:b6e02d6261d7 779
kishibekairohan 13:b6e02d6261d7 780 #if USE_PROCESS_NUM>7
kishibekairohan 13:b6e02d6261d7 781 static void Process7()
kishibekairohan 13:b6e02d6261d7 782 {
M_souta 24:370616a56815 783
kishibekairohan 12:c09b3e08a316 784 }
kishibekairohan 12:c09b3e08a316 785 #endif
kishibekairohan 12:c09b3e08a316 786
kishibekairohan 16:3f2c2d89372b 787 #if USE_PROCESS_NUM>8
kishibekairohan 13:b6e02d6261d7 788 static void Process8()
kishibekairohan 12:c09b3e08a316 789 {
M_souta 22:7d93f79a3686 790 if(controller->Button.A) {
M_souta 22:7d93f79a3686 791 rotaconSampling.start();
M_souta 22:7d93f79a3686 792 PIDflag = true;
M_souta 22:7d93f79a3686 793
M_souta 22:7d93f79a3686 794 //linePara_U = LineHub::GetPara(0);
M_souta 22:7d93f79a3686 795 //linePara_B = LineHub::GetPara(3);
M_souta 22:7d93f79a3686 796
M_souta 22:7d93f79a3686 797
M_souta 22:7d93f79a3686 798 pulsePV[FL] = encoder[FL].getPulses();
M_souta 22:7d93f79a3686 799 pulsePV[BL] = encoder[BL].getPulses();
M_souta 22:7d93f79a3686 800 pulsePV[BR] = encoder[BR].getPulses();
M_souta 22:7d93f79a3686 801 pulsePV[FR] = encoder[FR].getPulses();
M_souta 22:7d93f79a3686 802
M_souta 22:7d93f79a3686 803
M_souta 22:7d93f79a3686 804 for (int i = 0; i < 4; i++) {
M_souta 22:7d93f79a3686 805 timeCV[i] = timePV[i];
M_souta 22:7d93f79a3686 806 timePV[i] = rotaconSampling.read();
M_souta 22:7d93f79a3686 807 tireProcessRPM[i] = (pulsePV[i] - pulseCV[i])/ (float)(256 * 2) / (timePV[i] - timeCV[i]) * 60;
M_souta 22:7d93f79a3686 808 pulseCV[i] = pulsePV[i];
M_souta 22:7d93f79a3686 809 }
M_souta 22:7d93f79a3686 810
M_souta 22:7d93f79a3686 811 move.Vx = 0.5;
M_souta 22:7d93f79a3686 812 move.Vy = 0.5;
M_souta 22:7d93f79a3686 813 move.Va = 0;
M_souta 22:7d93f79a3686 814
M_souta 22:7d93f79a3686 815 correction_LT.Vx = 0; //0.1 * linePara_U;
M_souta 22:7d93f79a3686 816 correction_LT.Vy = 0;
M_souta 22:7d93f79a3686 817 correction_LT.Va = 0;
M_souta 22:7d93f79a3686 818
M_souta 22:7d93f79a3686 819 synthetic.Vx = move.Vx + correction_LT.Vx;
M_souta 22:7d93f79a3686 820 synthetic.Vy = move.Vy + correction_LT.Vy;
M_souta 22:7d93f79a3686 821 synthetic.Va = move.Va + correction_LT.Va;
M_souta 22:7d93f79a3686 822
M_souta 22:7d93f79a3686 823 sita = 0;
M_souta 22:7d93f79a3686 824
M_souta 22:7d93f79a3686 825 //タイヤの目標速度算出
M_souta 22:7d93f79a3686 826 float sinR = 0.7071 * (float)sin(sita);
M_souta 22:7d93f79a3686 827 float cosR = 0.7071 * (float)cos(sita);
M_souta 22:7d93f79a3686 828 float nv = (60 * 1000) / ( 2.00 * PI * tireR);
M_souta 22:7d93f79a3686 829 tireTargetRPM[FL] = ((+ synthetic.Vx * (sinR - cosR)) - (synthetic.Vy * (sinR + cosR)) + (ucR * synthetic.Va)) * nv;
M_souta 22:7d93f79a3686 830 tireTargetRPM[BL] = ((+ synthetic.Vx * (sinR + cosR)) + (synthetic.Vy * (sinR - cosR)) + (ucR * synthetic.Va)) * nv;
M_souta 22:7d93f79a3686 831 tireTargetRPM[BR] = ((- synthetic.Vx * (sinR - cosR)) + (synthetic.Vy * (sinR + cosR)) + (ucR * synthetic.Va)) * nv;
M_souta 22:7d93f79a3686 832 tireTargetRPM[FR] = ((- synthetic.Vx * (sinR + cosR)) - (synthetic.Vy * (sinR - cosR)) + (ucR * synthetic.Va)) * nv;
M_souta 22:7d93f79a3686 833
M_souta 22:7d93f79a3686 834 //pc.printf("process : %f target : %f\n\r",tireProcessRPM[0],tireTargetRPM[0]);
M_souta 22:7d93f79a3686 835
M_souta 22:7d93f79a3686 836 //PIDによるPWM算出
M_souta 22:7d93f79a3686 837
M_souta 22:7d93f79a3686 838 //モータの駆動
M_souta 22:7d93f79a3686 839 for (int i = 0; i < 4; i++) {
M_souta 22:7d93f79a3686 840 if (tirePWM[i] > 255){
M_souta 22:7d93f79a3686 841 tirePWM[i] = 255;
M_souta 22:7d93f79a3686 842 } else if (tirePWM[i] < -255) {
M_souta 22:7d93f79a3686 843 tirePWM[i] = -255;
M_souta 22:7d93f79a3686 844 }
M_souta 22:7d93f79a3686 845 }
M_souta 22:7d93f79a3686 846
M_souta 22:7d93f79a3686 847 for(int i = 0;i < 4;i++){
M_souta 22:7d93f79a3686 848 motor[i].dir = SetStatus(tirePWM[i]);
M_souta 22:7d93f79a3686 849 motor[i].pwm = SetPWM(tirePWM[i]);
M_souta 22:7d93f79a3686 850 }
M_souta 22:7d93f79a3686 851 } else {
M_souta 22:7d93f79a3686 852 PIDflag = false;
M_souta 22:7d93f79a3686 853 rotaconSampling.stop();
M_souta 22:7d93f79a3686 854 rotaconSampling.reset();
M_souta 22:7d93f79a3686 855 for(int i = 0;i < 4;i++){
M_souta 22:7d93f79a3686 856 encoder[i].reset();
M_souta 22:7d93f79a3686 857 pulsePV[i] = 0;
M_souta 22:7d93f79a3686 858 pulseCV[i] = 0;
M_souta 22:7d93f79a3686 859 timePV[i] = 0;
M_souta 22:7d93f79a3686 860 timeCV[i] = 0;
M_souta 22:7d93f79a3686 861 tirePWM[i] = 0;
M_souta 22:7d93f79a3686 862 motor[i].dir = SetStatus(tirePWM[i]);
M_souta 22:7d93f79a3686 863 motor[i].pwm = SetPWM(tirePWM[i]);
M_souta 22:7d93f79a3686 864 }
M_souta 22:7d93f79a3686 865 }
kishibekairohan 12:c09b3e08a316 866 }
kishibekairohan 12:c09b3e08a316 867 #endif
kishibekairohan 12:c09b3e08a316 868
kishibekairohan 13:b6e02d6261d7 869 #if USE_PROCESS_NUM>9
kishibekairohan 13:b6e02d6261d7 870 static void Process9()
kishibekairohan 12:c09b3e08a316 871 {
kishibekairohan 2:c015739085d3 872
t_yamamoto 0:669ef71cba68 873 }
t_yamamoto 0:669ef71cba68 874 #endif
t_yamamoto 0:669ef71cba68 875 #endif
t_yamamoto 0:669ef71cba68 876 #pragma endregion PROCESS
t_yamamoto 0:669ef71cba68 877
t_yamamoto 0:669ef71cba68 878 static void AllActuatorReset()
t_yamamoto 0:669ef71cba68 879 {
t_yamamoto 0:669ef71cba68 880
t_yamamoto 0:669ef71cba68 881 #ifdef USE_SOLENOID
t_yamamoto 0:669ef71cba68 882 solenoid.all = ALL_SOLENOID_OFF;
t_yamamoto 0:669ef71cba68 883 #endif
t_yamamoto 0:669ef71cba68 884
t_yamamoto 0:669ef71cba68 885 #ifdef USE_MOTOR
t_yamamoto 0:669ef71cba68 886 for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++)
t_yamamoto 0:669ef71cba68 887 {
t_yamamoto 0:669ef71cba68 888 motor[i].dir = FREE;
t_yamamoto 0:669ef71cba68 889 motor[i].pwm = 0;
t_yamamoto 0:669ef71cba68 890 }
t_yamamoto 0:669ef71cba68 891 #endif
t_yamamoto 0:669ef71cba68 892 }
t_yamamoto 0:669ef71cba68 893
kishibekairohan 16:3f2c2d89372b 894 void BuzzerTimer_func(){
kishibekairohan 16:3f2c2d89372b 895 buzzer = !buzzer;
kishibekairohan 17:50dc4b449e69 896 //LED_DEBUG0 = !LED_DEBUG0;
kishibekairohan 11:028a150943b5 897 }
kishibekairohan 11:028a150943b5 898
kishibekairohan 11:028a150943b5 899 void TapeLedEms_func() {
kishibekairohan 16:3f2c2d89372b 900 sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
kishibekairohan 9:f93fc79a49ea 901 }
kishibekairohan 16:3f2c2d89372b 902
kishibekairohan 16:3f2c2d89372b 903 #pragma region USER-DEFINED-FUNCTIONS
M_souta 22:7d93f79a3686 904 void tirePID() {
M_souta 22:7d93f79a3686 905 if(PIDflag == true) {
M_souta 22:7d93f79a3686 906 //加算するPID値の算出
M_souta 22:7d93f79a3686 907 rotaconPID[0].SetPV(tireProcessRPM[FL],tireTargetRPM[FL]);
M_souta 22:7d93f79a3686 908 rotaconPID[1].SetPV(tireProcessRPM[BL],tireTargetRPM[BL]);
M_souta 22:7d93f79a3686 909 rotaconPID[2].SetPV(tireProcessRPM[FR],tireTargetRPM[FR]);
M_souta 22:7d93f79a3686 910 rotaconPID[3].SetPV(tireProcessRPM[BR],tireTargetRPM[BR]);
M_souta 22:7d93f79a3686 911 //PID値の加算
M_souta 22:7d93f79a3686 912 tirePWM[FL] += rotaconPID[0].GetMV();
M_souta 22:7d93f79a3686 913 tirePWM[BL] += rotaconPID[1].GetMV();
M_souta 22:7d93f79a3686 914 tirePWM[FR] += rotaconPID[2].GetMV();
M_souta 22:7d93f79a3686 915 tirePWM[BR] += rotaconPID[3].GetMV();
M_souta 22:7d93f79a3686 916 }
M_souta 22:7d93f79a3686 917 }
kishibekairohan 16:3f2c2d89372b 918
M_souta 22:7d93f79a3686 919 int lineCast(char k) {
M_souta 22:7d93f79a3686 920 int l;
M_souta 22:7d93f79a3686 921 switch(k) {
M_souta 22:7d93f79a3686 922 case 255:
M_souta 22:7d93f79a3686 923 l = -1;
M_souta 22:7d93f79a3686 924 break;
M_souta 22:7d93f79a3686 925 case 254:
M_souta 22:7d93f79a3686 926 l = -2;
M_souta 22:7d93f79a3686 927 break;
M_souta 22:7d93f79a3686 928 case 253:
M_souta 22:7d93f79a3686 929 l = -3;
M_souta 22:7d93f79a3686 930 break;
M_souta 22:7d93f79a3686 931 default:
M_souta 22:7d93f79a3686 932 l = k;
M_souta 22:7d93f79a3686 933 }
M_souta 22:7d93f79a3686 934 return l;
M_souta 22:7d93f79a3686 935 }
kishibekairohan 16:3f2c2d89372b 936
t_yamamoto 0:669ef71cba68 937 #pragma endregion