lknds
Dependencies: mbed TrapezoidControl Pulse QEI
System/Process/Process.cpp@25:d367d1e7a153, 2019-09-27 (annotated)
- Committer:
- Ryosei
- Date:
- Fri Sep 27 05:19:58 2019 +0000
- Revision:
- 25:d367d1e7a153
- Parent:
- 24:370616a56815
dskb
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Ryosei | 25:d367d1e7a153 | 1 | /////////////////////////////////////////////////////////////////////// |
kishibekairohan | 13:b6e02d6261d7 | 2 | |
Ryosei | 25:d367d1e7a153 | 3 | /* |
Ryosei | 25:d367d1e7a153 | 4 | 三浦先輩へ |
Ryosei | 25:d367d1e7a153 | 5 | 主に変えたところ |
Ryosei | 25:d367d1e7a153 | 6 | ・超音波センサーの値取得の関数化 |
Ryosei | 25:d367d1e7a153 | 7 | ・PulseInOutライブラリを導入 |
Ryosei | 25:d367d1e7a153 | 8 | ・LedOut()関数を追加 |
Ryosei | 25:d367d1e7a153 | 9 | |
Ryosei | 25:d367d1e7a153 | 10 | の3つです。他にも変えたところがあるかもしれないので、確認お願いします。 |
Ryosei | 25:d367d1e7a153 | 11 | また、ソレノイドハブとの通信は書いていません。洗濯ばさみの電磁弁はNRPの時の例のアレで動かしています。 |
Ryosei | 25:d367d1e7a153 | 12 | 普通にピン数が足りないため、ソレノイドハブでプログラムを動かすことを強く望みます。 |
Ryosei | 25:d367d1e7a153 | 13 | Process.hの確認もよくお願いします。 |
Ryosei | 25:d367d1e7a153 | 14 | LedOutですが、現在は |
Ryosei | 25:d367d1e7a153 | 15 | LedOut(0)=金色光りっぱなし |
Ryosei | 25:d367d1e7a153 | 16 | LedOut(1)=旧式の光り方 |
Ryosei | 25:d367d1e7a153 | 17 | LedOut(2)=ゾーン色で光りっぱなし |
Ryosei | 25:d367d1e7a153 | 18 | LedOut(3)=ゾーン色でふわんふわん |
Ryosei | 25:d367d1e7a153 | 19 | となっています。希望があれば追加します。無ければ適当に引数が増えるごとに光り方をうざくしていきます。 |
Ryosei | 25:d367d1e7a153 | 20 | 温度センサーですが、調子が悪いため、直で温度の変数を設定してください。 |
Ryosei | 25:d367d1e7a153 | 21 | //////////////////////////////////////////////////////////////////////////////////////// |
Ryosei | 25:d367d1e7a153 | 22 | */ |
Ryosei | 25:d367d1e7a153 | 23 | //////////////////////////////// |
Ryosei | 25:d367d1e7a153 | 24 | /* |
Ryosei | 25:d367d1e7a153 | 25 | 2019/09/24 |
Ryosei | 25:d367d1e7a153 | 26 | Process.cpp |
Ryosei | 25:d367d1e7a153 | 27 | Auther:Miyagawa Ryosei |
Ryosei | 25:d367d1e7a153 | 28 | |
Ryosei | 25:d367d1e7a153 | 29 | */ |
Ryosei | 25:d367d1e7a153 | 30 | //////////////////////////////// |
t_yamamoto | 0:669ef71cba68 | 31 | #include "mbed.h" |
t_yamamoto | 0:669ef71cba68 | 32 | #include "Process.h" |
t_yamamoto | 0:669ef71cba68 | 33 | |
Ryosei | 25:d367d1e7a153 | 34 | #include "Pulse.h"////////////こいつ |
Ryosei | 25:d367d1e7a153 | 35 | |
Ryosei | 25:d367d1e7a153 | 36 | |
7ka884 | 4:ba9df71868df | 37 | #include "../../CommonLibraries/PID/PID.h" |
t_yamamoto | 0:669ef71cba68 | 38 | #include "../../Communication/RS485/ActuatorHub/ActuatorHub.h" |
M_souta | 21:e3b58d675c1c | 39 | #include "../../Communication/RS485/LineHub/LineHub.h" |
t_yamamoto | 0:669ef71cba68 | 40 | #include "../../Communication/Controller/Controller.h" |
t_yamamoto | 0:669ef71cba68 | 41 | #include "../../Input/ExternalInt/ExternalInt.h" |
t_yamamoto | 0:669ef71cba68 | 42 | #include "../../Input/Switch/Switch.h" |
kishibekairohan | 16:3f2c2d89372b | 43 | #include "../../Input/Encoder/Encoder.h" |
Ryosei | 25:d367d1e7a153 | 44 | //#include "../../Input/Ultrasonic/Ultrasonic.h" |
t_yamamoto | 0:669ef71cba68 | 45 | #include "../../LED/LED.h" |
t_yamamoto | 0:669ef71cba68 | 46 | #include "../../Safty/Safty.h" |
t_yamamoto | 0:669ef71cba68 | 47 | #include "../Using.h" |
t_yamamoto | 0:669ef71cba68 | 48 | |
t_yamamoto | 0:669ef71cba68 | 49 | using namespace SWITCH; |
7ka884 | 4:ba9df71868df | 50 | using namespace PID_SPACE; |
kishibekairohan | 16:3f2c2d89372b | 51 | using namespace ENCODER; |
Ryosei | 25:d367d1e7a153 | 52 | //using namespace ULTRSONIC; |
M_souta | 21:e3b58d675c1c | 53 | using namespace LINEHUB; |
t_yamamoto | 0:669ef71cba68 | 54 | |
M_souta | 22:7d93f79a3686 | 55 | |
t_yamamoto | 0:669ef71cba68 | 56 | static CONTROLLER::ControllerData *controller; |
t_yamamoto | 0:669ef71cba68 | 57 | ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM]; |
t_yamamoto | 0:669ef71cba68 | 58 | ACTUATORHUB::SOLENOID::SolenoidStatus solenoid; |
t_yamamoto | 0:669ef71cba68 | 59 | |
t_yamamoto | 0:669ef71cba68 | 60 | static bool lock; |
t_yamamoto | 0:669ef71cba68 | 61 | static bool processChangeComp; |
t_yamamoto | 0:669ef71cba68 | 62 | static int current; |
t_yamamoto | 0:669ef71cba68 | 63 | |
t_yamamoto | 0:669ef71cba68 | 64 | static void AllActuatorReset(); |
t_yamamoto | 0:669ef71cba68 | 65 | |
t_yamamoto | 0:669ef71cba68 | 66 | #ifdef USE_SUBPROCESS |
t_yamamoto | 0:669ef71cba68 | 67 | static void (*Process[USE_PROCESS_NUM])(void); |
t_yamamoto | 0:669ef71cba68 | 68 | #endif |
t_yamamoto | 0:669ef71cba68 | 69 | |
t_yamamoto | 0:669ef71cba68 | 70 | #pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE |
t_yamamoto | 0:669ef71cba68 | 71 | |
t_yamamoto | 0:669ef71cba68 | 72 | /*Replace here with the definition code of your variables.*/ |
Ryosei | 25:d367d1e7a153 | 73 | /* |
M_souta | 22:7d93f79a3686 | 74 | USS ultrasonic[] = { |
M_souta | 22:7d93f79a3686 | 75 | USS(ECHO_0,TRIG_0,TEMP), |
M_souta | 22:7d93f79a3686 | 76 | USS(ECHO_1,TRIG_1,TEMP), |
M_souta | 22:7d93f79a3686 | 77 | }; |
Ryosei | 25:d367d1e7a153 | 78 | */ |
M_souta | 22:7d93f79a3686 | 79 | Serial pc(USBTX, USBRX); |
kishibekairohan | 7:e88c5d47a3be | 80 | |
kishibekairohan | 16:3f2c2d89372b | 81 | //**************Buzzer**************** |
kishibekairohan | 11:028a150943b5 | 82 | //DigitalOut buzzer(BUZZER_PIN); |
kishibekairohan | 9:f93fc79a49ea | 83 | void BuzzerTimer_func(); |
kishibekairohan | 9:f93fc79a49ea | 84 | Ticker BuzzerTimer; |
kishibekairohan | 16:3f2c2d89372b | 85 | bool EMGflag = false; |
kishibekairohan | 16:3f2c2d89372b | 86 | PwmOut buzzer(BUZZER_PIN); |
kishibekairohan | 16:3f2c2d89372b | 87 | //**************Buzzer**************** |
kishibekairohan | 11:028a150943b5 | 88 | |
kishibekairohan | 11:028a150943b5 | 89 | //************TapeLed***************** |
kishibekairohan | 11:028a150943b5 | 90 | void TapeLedEms_func(); |
kishibekairohan | 11:028a150943b5 | 91 | TapeLedData tapeLED; |
kishibekairohan | 11:028a150943b5 | 92 | TapeLedData sendLedData; |
kishibekairohan | 11:028a150943b5 | 93 | TapeLED_Mode ledMode = Normal; |
kishibekairohan | 11:028a150943b5 | 94 | Ticker tapeLedTimer; |
kishibekairohan | 11:028a150943b5 | 95 | //************TapaLed***************** |
kishibekairohan | 16:3f2c2d89372b | 96 | |
M_souta | 22:7d93f79a3686 | 97 | //*************** lift *************** |
M_souta | 22:7d93f79a3686 | 98 | #define LOWER 1 |
M_souta | 22:7d93f79a3686 | 99 | #define MIDDLRE 2 |
M_souta | 22:7d93f79a3686 | 100 | #define UPPER 3 |
M_souta | 22:7d93f79a3686 | 101 | uint8_t liftState = LOWER; |
M_souta | 22:7d93f79a3686 | 102 | bool moving = false; |
M_souta | 22:7d93f79a3686 | 103 | bool switchFlag_LB = false; |
M_souta | 22:7d93f79a3686 | 104 | bool switchFlag_RB = false; |
M_souta | 22:7d93f79a3686 | 105 | |
M_souta | 22:7d93f79a3686 | 106 | //*************** lift *************** |
M_souta | 22:7d93f79a3686 | 107 | |
M_souta | 22:7d93f79a3686 | 108 | //*************tire************* |
M_souta | 22:7d93f79a3686 | 109 | PID rotaconPID[] = { |
Ryosei | 25:d367d1e7a153 | 110 | PID(0.0001,-1,1,0.05,0,0), //LF |
Ryosei | 25:d367d1e7a153 | 111 | PID(0.0001,-1,1,0.05,0,0), //LB |
Ryosei | 25:d367d1e7a153 | 112 | PID(0.0001,-1,1,0.05,0,0), //RB |
Ryosei | 25:d367d1e7a153 | 113 | PID(0.0001,-1,1,0.05,0,0), //RF |
M_souta | 22:7d93f79a3686 | 114 | }; |
M_souta | 22:7d93f79a3686 | 115 | |
M_souta | 22:7d93f79a3686 | 116 | #define FL 0 |
M_souta | 22:7d93f79a3686 | 117 | #define BL 1 |
M_souta | 22:7d93f79a3686 | 118 | #define BR 2 |
M_souta | 22:7d93f79a3686 | 119 | #define FR 3 |
M_souta | 22:7d93f79a3686 | 120 | |
M_souta | 22:7d93f79a3686 | 121 | #define PI 3.141592 |
M_souta | 22:7d93f79a3686 | 122 | |
M_souta | 22:7d93f79a3686 | 123 | const float tireR = 101.6; //タイヤの半径 |
M_souta | 22:7d93f79a3686 | 124 | const float ucR = 420.0; //中心からのタイヤの距離 |
M_souta | 22:7d93f79a3686 | 125 | |
M_souta | 22:7d93f79a3686 | 126 | typedef struct { |
Ryosei | 25:d367d1e7a153 | 127 | float Vx; //X方向の速度 |
Ryosei | 25:d367d1e7a153 | 128 | float Vy; //Y方向の速度 |
Ryosei | 25:d367d1e7a153 | 129 | float Va; //角速度 |
M_souta | 22:7d93f79a3686 | 130 | } Vvector; |
M_souta | 22:7d93f79a3686 | 131 | |
M_souta | 22:7d93f79a3686 | 132 | Vvector move; //進む速度 |
M_souta | 22:7d93f79a3686 | 133 | Vvector correction_LT; //ライントレースの補正速度 |
M_souta | 22:7d93f79a3686 | 134 | Vvector synthetic; //合成速度 |
M_souta | 22:7d93f79a3686 | 135 | |
M_souta | 22:7d93f79a3686 | 136 | float sita = 0; |
M_souta | 22:7d93f79a3686 | 137 | |
M_souta | 22:7d93f79a3686 | 138 | bool PIDflag = false; |
M_souta | 22:7d93f79a3686 | 139 | |
M_souta | 22:7d93f79a3686 | 140 | int linePara[8]; |
M_souta | 22:7d93f79a3686 | 141 | int linePara_U; |
M_souta | 22:7d93f79a3686 | 142 | int linePara_B; |
M_souta | 22:7d93f79a3686 | 143 | int linePara_L; |
M_souta | 22:7d93f79a3686 | 144 | int linePara_R; |
M_souta | 22:7d93f79a3686 | 145 | |
M_souta | 22:7d93f79a3686 | 146 | #define FL 0 |
M_souta | 22:7d93f79a3686 | 147 | #define BL 1 |
M_souta | 22:7d93f79a3686 | 148 | #define BR 2 |
M_souta | 22:7d93f79a3686 | 149 | #define FR 3 |
M_souta | 22:7d93f79a3686 | 150 | |
M_souta | 22:7d93f79a3686 | 151 | float tireProcessRPM[4]; |
M_souta | 22:7d93f79a3686 | 152 | float tireTargetMaxRPM[4]; |
M_souta | 22:7d93f79a3686 | 153 | float tireTargetRPM[4]; |
M_souta | 22:7d93f79a3686 | 154 | |
M_souta | 22:7d93f79a3686 | 155 | float tirePWM[4]; |
M_souta | 22:7d93f79a3686 | 156 | |
M_souta | 22:7d93f79a3686 | 157 | float timePV[4]; |
M_souta | 22:7d93f79a3686 | 158 | float timeCV[4]; |
M_souta | 22:7d93f79a3686 | 159 | float pulsePV[4]; |
M_souta | 22:7d93f79a3686 | 160 | float pulseCV[4]; |
M_souta | 22:7d93f79a3686 | 161 | |
M_souta | 22:7d93f79a3686 | 162 | void tirePID(); |
M_souta | 22:7d93f79a3686 | 163 | int lineCast(char k); |
M_souta | 22:7d93f79a3686 | 164 | |
M_souta | 22:7d93f79a3686 | 165 | Timer rotaconSampling; |
M_souta | 22:7d93f79a3686 | 166 | Ticker rotaconPIDtimer; |
M_souta | 22:7d93f79a3686 | 167 | |
M_souta | 22:7d93f79a3686 | 168 | bool countFlag; |
M_souta | 22:7d93f79a3686 | 169 | //*************tire**************// |
M_souta | 22:7d93f79a3686 | 170 | |
M_souta | 22:7d93f79a3686 | 171 | // ************* Line ************** // |
M_souta | 22:7d93f79a3686 | 172 | |
M_souta | 22:7d93f79a3686 | 173 | float pw = 0; |
M_souta | 22:7d93f79a3686 | 174 | int lineFase = 0; |
M_souta | 22:7d93f79a3686 | 175 | bool lineCheck = false; |
M_souta | 22:7d93f79a3686 | 176 | int linePWM; |
M_souta | 22:7d93f79a3686 | 177 | int adj_F; |
M_souta | 22:7d93f79a3686 | 178 | int adj_B; |
M_souta | 22:7d93f79a3686 | 179 | |
M_souta | 22:7d93f79a3686 | 180 | int mode = 0; |
M_souta | 22:7d93f79a3686 | 181 | |
M_souta | 22:7d93f79a3686 | 182 | int lineCount = 0; |
M_souta | 22:7d93f79a3686 | 183 | |
M_souta | 22:7d93f79a3686 | 184 | // ************* Line ************** // |
M_souta | 22:7d93f79a3686 | 185 | |
Ryosei | 25:d367d1e7a153 | 186 | const int omni[15][15] = { |
Ryosei | 25:d367d1e7a153 | 187 | { 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255 }, |
Ryosei | 25:d367d1e7a153 | 188 | { -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255 }, |
Ryosei | 25:d367d1e7a153 | 189 | { -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255 }, |
Ryosei | 25:d367d1e7a153 | 190 | { -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255 }, |
Ryosei | 25:d367d1e7a153 | 191 | { -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255 }, |
Ryosei | 25:d367d1e7a153 | 192 | { -130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255 }, |
Ryosei | 25:d367d1e7a153 | 193 | { -187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255 }, |
Ryosei | 25:d367d1e7a153 | 194 | { -255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255 }, |
Ryosei | 25:d367d1e7a153 | 195 | { -255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187 }, |
Ryosei | 25:d367d1e7a153 | 196 | { -255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130 }, |
Ryosei | 25:d367d1e7a153 | 197 | { -255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83 }, |
Ryosei | 25:d367d1e7a153 | 198 | { -255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47 }, |
Ryosei | 25:d367d1e7a153 | 199 | { -255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21 }, |
Ryosei | 25:d367d1e7a153 | 200 | { -255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5 }, |
Ryosei | 25:d367d1e7a153 | 201 | { -255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0 } |
M_souta | 21:e3b58d675c1c | 202 | }; |
M_souta | 21:e3b58d675c1c | 203 | |
M_souta | 21:e3b58d675c1c | 204 | const int curve[15] = { -204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204 }; |
M_souta | 21:e3b58d675c1c | 205 | |
M_souta | 21:e3b58d675c1c | 206 | uint8_t SetStatus(int); |
Ryosei | 25:d367d1e7a153 | 207 | uint8_t SetStatus(int pwmVal) |
Ryosei | 25:d367d1e7a153 | 208 | { |
Ryosei | 25:d367d1e7a153 | 209 | if (pwmVal < 0) return BACK; |
Ryosei | 25:d367d1e7a153 | 210 | else if (pwmVal > 0) return FOR; |
Ryosei | 25:d367d1e7a153 | 211 | else if (pwmVal == 0) return BRAKE; |
Ryosei | 25:d367d1e7a153 | 212 | else return BRAKE; |
M_souta | 20:eae8c84f318c | 213 | } |
M_souta | 21:e3b58d675c1c | 214 | uint8_t SetPWM(int); |
Ryosei | 25:d367d1e7a153 | 215 | uint8_t SetPWM(int pwmVal) |
Ryosei | 25:d367d1e7a153 | 216 | { |
Ryosei | 25:d367d1e7a153 | 217 | if (pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255; |
Ryosei | 25:d367d1e7a153 | 218 | else return abs(pwmVal); |
M_souta | 21:e3b58d675c1c | 219 | } |
Ryosei | 25:d367d1e7a153 | 220 | //*********Ultra-Sound sensor*************** |
Ryosei | 25:d367d1e7a153 | 221 | AnalogIn Temp(TEMP); |
Ryosei | 25:d367d1e7a153 | 222 | //double temp=( Temp.read()* 3.3 - 0.6) / 0.01; |
Ryosei | 25:d367d1e7a153 | 223 | PulseInOut Echo0(ECHO_0); |
Ryosei | 25:d367d1e7a153 | 224 | PulseInOut Echo1(ECHO_1); |
Ryosei | 25:d367d1e7a153 | 225 | PulseInOut Trig0(TRIG_0); |
Ryosei | 25:d367d1e7a153 | 226 | PulseInOut Trig1(TRIG_1); |
Ryosei | 25:d367d1e7a153 | 227 | double UltraRead(int num) |
Ryosei | 25:d367d1e7a153 | 228 | { |
Ryosei | 25:d367d1e7a153 | 229 | double Distance=0; |
Ryosei | 25:d367d1e7a153 | 230 | double Duration=0; |
Ryosei | 25:d367d1e7a153 | 231 | double temp=21;////////////////////////////////////////////////温度 |
Ryosei | 25:d367d1e7a153 | 232 | if(num==0) { |
Ryosei | 25:d367d1e7a153 | 233 | Trig0.write_us(1,10); |
Ryosei | 25:d367d1e7a153 | 234 | Duration=Echo0.read_high_us(5000); |
Ryosei | 25:d367d1e7a153 | 235 | } else if(num==1) { |
Ryosei | 25:d367d1e7a153 | 236 | Trig1.write_us(1,10); |
Ryosei | 25:d367d1e7a153 | 237 | Duration=Echo1.read_high_us(5000); |
Ryosei | 25:d367d1e7a153 | 238 | } |
Ryosei | 25:d367d1e7a153 | 239 | if(Duration>0) { |
Ryosei | 25:d367d1e7a153 | 240 | Duration=Duration/2; |
Ryosei | 25:d367d1e7a153 | 241 | double sspead=331.5+0.6*temp; |
Ryosei | 25:d367d1e7a153 | 242 | Distance=Duration*sspead*100/1000000; |
Ryosei | 25:d367d1e7a153 | 243 | } else { |
Ryosei | 25:d367d1e7a153 | 244 | return 0; |
Ryosei | 25:d367d1e7a153 | 245 | } |
Ryosei | 25:d367d1e7a153 | 246 | return Distance; |
Ryosei | 25:d367d1e7a153 | 247 | } |
Ryosei | 25:d367d1e7a153 | 248 | //*********Ultra-Sound sensor*************** |
Ryosei | 25:d367d1e7a153 | 249 | |
Ryosei | 25:d367d1e7a153 | 250 | //*********Tape LED************************** |
Ryosei | 25:d367d1e7a153 | 251 | DigitalOut select1(SELECT_1); |
Ryosei | 25:d367d1e7a153 | 252 | DigitalOut select2(SELECT_2); |
Ryosei | 25:d367d1e7a153 | 253 | DigitalOut select3(SELECT_3); |
Ryosei | 25:d367d1e7a153 | 254 | void LedOut(int num) |
Ryosei | 25:d367d1e7a153 | 255 | { |
Ryosei | 25:d367d1e7a153 | 256 | int selectnum[8][3]= { |
Ryosei | 25:d367d1e7a153 | 257 | {0,0,0}, |
Ryosei | 25:d367d1e7a153 | 258 | {0,0,1}, |
Ryosei | 25:d367d1e7a153 | 259 | {0,1,0}, |
Ryosei | 25:d367d1e7a153 | 260 | {0,1,1}, |
Ryosei | 25:d367d1e7a153 | 261 | {1,0,0}, |
Ryosei | 25:d367d1e7a153 | 262 | {1,0,1}, |
Ryosei | 25:d367d1e7a153 | 263 | {1,1,0}, |
Ryosei | 25:d367d1e7a153 | 264 | {1,1,1} |
Ryosei | 25:d367d1e7a153 | 265 | }; |
Ryosei | 25:d367d1e7a153 | 266 | select1=selectnum[num][0]; |
Ryosei | 25:d367d1e7a153 | 267 | select2=selectnum[num][1]; |
Ryosei | 25:d367d1e7a153 | 268 | select3=selectnum[num][2]; |
Ryosei | 25:d367d1e7a153 | 269 | } |
Ryosei | 25:d367d1e7a153 | 270 | //*********Tape LED************************** |
Ryosei | 25:d367d1e7a153 | 271 | |
M_souta | 18:c694bae76e51 | 272 | |
t_yamamoto | 0:669ef71cba68 | 273 | #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE |
t_yamamoto | 0:669ef71cba68 | 274 | |
t_yamamoto | 0:669ef71cba68 | 275 | #ifdef USE_SUBPROCESS |
t_yamamoto | 0:669ef71cba68 | 276 | #if USE_PROCESS_NUM>0 |
t_yamamoto | 0:669ef71cba68 | 277 | static void Process0(void); |
t_yamamoto | 0:669ef71cba68 | 278 | #endif |
t_yamamoto | 0:669ef71cba68 | 279 | #if USE_PROCESS_NUM>1 |
t_yamamoto | 0:669ef71cba68 | 280 | static void Process1(void); |
t_yamamoto | 0:669ef71cba68 | 281 | #endif |
t_yamamoto | 0:669ef71cba68 | 282 | #if USE_PROCESS_NUM>2 |
t_yamamoto | 0:669ef71cba68 | 283 | static void Process2(void); |
t_yamamoto | 0:669ef71cba68 | 284 | #endif |
t_yamamoto | 0:669ef71cba68 | 285 | #if USE_PROCESS_NUM>3 |
t_yamamoto | 0:669ef71cba68 | 286 | static void Process3(void); |
t_yamamoto | 0:669ef71cba68 | 287 | #endif |
t_yamamoto | 0:669ef71cba68 | 288 | #if USE_PROCESS_NUM>4 |
t_yamamoto | 0:669ef71cba68 | 289 | static void Process4(void); |
t_yamamoto | 0:669ef71cba68 | 290 | #endif |
t_yamamoto | 0:669ef71cba68 | 291 | #if USE_PROCESS_NUM>5 |
t_yamamoto | 0:669ef71cba68 | 292 | static void Process5(void); |
t_yamamoto | 0:669ef71cba68 | 293 | #endif |
t_yamamoto | 0:669ef71cba68 | 294 | #if USE_PROCESS_NUM>6 |
t_yamamoto | 0:669ef71cba68 | 295 | static void Process6(void); |
t_yamamoto | 0:669ef71cba68 | 296 | #endif |
t_yamamoto | 0:669ef71cba68 | 297 | #if USE_PROCESS_NUM>7 |
t_yamamoto | 0:669ef71cba68 | 298 | static void Process7(void); |
t_yamamoto | 0:669ef71cba68 | 299 | #endif |
t_yamamoto | 0:669ef71cba68 | 300 | #if USE_PROCESS_NUM>8 |
t_yamamoto | 0:669ef71cba68 | 301 | static void Process8(void); |
t_yamamoto | 0:669ef71cba68 | 302 | #endif |
t_yamamoto | 0:669ef71cba68 | 303 | #if USE_PROCESS_NUM>9 |
t_yamamoto | 0:669ef71cba68 | 304 | static void Process9(void); |
t_yamamoto | 0:669ef71cba68 | 305 | #endif |
t_yamamoto | 0:669ef71cba68 | 306 | #endif |
t_yamamoto | 0:669ef71cba68 | 307 | |
t_yamamoto | 0:669ef71cba68 | 308 | void SystemProcessInitialize() |
t_yamamoto | 0:669ef71cba68 | 309 | { |
Ryosei | 25:d367d1e7a153 | 310 | #pragma region USER-DEFINED_VARIABLE_INIT |
Ryosei | 25:d367d1e7a153 | 311 | /*Replace here with the initialization code of your variables.*/ |
Ryosei | 25:d367d1e7a153 | 312 | rotaconPIDtimer.attach(tirePID,0.1); |
t_yamamoto | 0:669ef71cba68 | 313 | |
Ryosei | 25:d367d1e7a153 | 314 | #pragma endregion USER-DEFINED_VARIABLE_INIT |
Ryosei | 25:d367d1e7a153 | 315 | |
Ryosei | 25:d367d1e7a153 | 316 | lock = true; |
Ryosei | 25:d367d1e7a153 | 317 | processChangeComp = true; |
Ryosei | 25:d367d1e7a153 | 318 | current = DEFAULT_PROCESS; |
t_yamamoto | 0:669ef71cba68 | 319 | |
Ryosei | 25:d367d1e7a153 | 320 | #ifdef USE_SUBPROCESS |
Ryosei | 25:d367d1e7a153 | 321 | #if USE_PROCESS_NUM>0 |
Ryosei | 25:d367d1e7a153 | 322 | Process[0] = Process0; |
Ryosei | 25:d367d1e7a153 | 323 | #endif |
Ryosei | 25:d367d1e7a153 | 324 | #if USE_PROCESS_NUM>1 |
Ryosei | 25:d367d1e7a153 | 325 | Process[1] = Process1; |
Ryosei | 25:d367d1e7a153 | 326 | #endif |
Ryosei | 25:d367d1e7a153 | 327 | #if USE_PROCESS_NUM>2 |
Ryosei | 25:d367d1e7a153 | 328 | Process[2] = Process2; |
Ryosei | 25:d367d1e7a153 | 329 | #endif |
Ryosei | 25:d367d1e7a153 | 330 | #if USE_PROCESS_NUM>3 |
Ryosei | 25:d367d1e7a153 | 331 | Process[3] = Process3; |
Ryosei | 25:d367d1e7a153 | 332 | #endif |
Ryosei | 25:d367d1e7a153 | 333 | #if USE_PROCESS_NUM>4 |
Ryosei | 25:d367d1e7a153 | 334 | Process[4] = Process4; |
Ryosei | 25:d367d1e7a153 | 335 | #endif |
Ryosei | 25:d367d1e7a153 | 336 | #if USE_PROCESS_NUM>5 |
Ryosei | 25:d367d1e7a153 | 337 | Process[5] = Process5; |
Ryosei | 25:d367d1e7a153 | 338 | #endif |
Ryosei | 25:d367d1e7a153 | 339 | #if USE_PROCESS_NUM>6 |
Ryosei | 25:d367d1e7a153 | 340 | Process[6] = Process6; |
Ryosei | 25:d367d1e7a153 | 341 | #endif |
Ryosei | 25:d367d1e7a153 | 342 | #if USE_PROCESS_NUM>7 |
Ryosei | 25:d367d1e7a153 | 343 | Process[7] = Process7; |
Ryosei | 25:d367d1e7a153 | 344 | #endif |
Ryosei | 25:d367d1e7a153 | 345 | #if USE_PROCESS_NUM>8 |
Ryosei | 25:d367d1e7a153 | 346 | Process[8] = Process8; |
Ryosei | 25:d367d1e7a153 | 347 | #endif |
Ryosei | 25:d367d1e7a153 | 348 | #if USE_PROCESS_NUM>9 |
Ryosei | 25:d367d1e7a153 | 349 | Process[9] = Process9; |
Ryosei | 25:d367d1e7a153 | 350 | #endif |
Ryosei | 25:d367d1e7a153 | 351 | #endif |
t_yamamoto | 0:669ef71cba68 | 352 | } |
t_yamamoto | 0:669ef71cba68 | 353 | |
t_yamamoto | 0:669ef71cba68 | 354 | static void SystemProcessUpdate() |
t_yamamoto | 0:669ef71cba68 | 355 | { |
Ryosei | 25:d367d1e7a153 | 356 | #ifdef USE_SUBPROCESS |
Ryosei | 25:d367d1e7a153 | 357 | if(controller->Button.HOME) lock = false; |
Ryosei | 25:d367d1e7a153 | 358 | |
Ryosei | 25:d367d1e7a153 | 359 | if(controller->Button.START && processChangeComp) { |
Ryosei | 25:d367d1e7a153 | 360 | current++; |
Ryosei | 25:d367d1e7a153 | 361 | if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM; |
Ryosei | 25:d367d1e7a153 | 362 | processChangeComp = false; |
Ryosei | 25:d367d1e7a153 | 363 | } else if(controller->Button.SELECT && processChangeComp) { |
Ryosei | 25:d367d1e7a153 | 364 | current--; |
Ryosei | 25:d367d1e7a153 | 365 | if (current < 0) current = 0; |
Ryosei | 25:d367d1e7a153 | 366 | processChangeComp = false; |
Ryosei | 25:d367d1e7a153 | 367 | } else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true; |
Ryosei | 25:d367d1e7a153 | 368 | #endif |
t_yamamoto | 0:669ef71cba68 | 369 | |
Ryosei | 25:d367d1e7a153 | 370 | #ifdef USE_MOTOR |
Ryosei | 25:d367d1e7a153 | 371 | ACTUATORHUB::MOTOR::Motor::Update(motor); |
Ryosei | 25:d367d1e7a153 | 372 | #endif |
Ryosei | 25:d367d1e7a153 | 373 | |
Ryosei | 25:d367d1e7a153 | 374 | #ifdef USE_SOLENOID |
Ryosei | 25:d367d1e7a153 | 375 | ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid); |
Ryosei | 25:d367d1e7a153 | 376 | #endif |
Ryosei | 25:d367d1e7a153 | 377 | |
Ryosei | 25:d367d1e7a153 | 378 | #ifdef USE_RS485 |
Ryosei | 25:d367d1e7a153 | 379 | ACTUATORHUB::ActuatorHub::Update(); |
Ryosei | 25:d367d1e7a153 | 380 | //LINEHUB::LineHub::Update(); |
Ryosei | 25:d367d1e7a153 | 381 | #endif |
Ryosei | 25:d367d1e7a153 | 382 | |
t_yamamoto | 0:669ef71cba68 | 383 | } |
t_yamamoto | 0:669ef71cba68 | 384 | |
kishibekairohan | 2:c015739085d3 | 385 | |
Ryosei | 25:d367d1e7a153 | 386 | double Ult_left=UltraRead(0);///////////////////////////////////////////left sensor |
Ryosei | 25:d367d1e7a153 | 387 | double Ult_right=UltraRead(1);//////////////////////////////////////////right sensor |
Ryosei | 25:d367d1e7a153 | 388 | |
kishibekairohan | 2:c015739085d3 | 389 | |
t_yamamoto | 0:669ef71cba68 | 390 | void SystemProcess() |
t_yamamoto | 0:669ef71cba68 | 391 | { |
Ryosei | 25:d367d1e7a153 | 392 | SystemProcessInitialize(); |
Ryosei | 25:d367d1e7a153 | 393 | |
Ryosei | 25:d367d1e7a153 | 394 | while(1) { |
Ryosei | 25:d367d1e7a153 | 395 | int g[8]; |
Ryosei | 25:d367d1e7a153 | 396 | for(int i = 0; i < 8; i++) { |
Ryosei | 25:d367d1e7a153 | 397 | g[i] = lineCast(LineHub::GetPara(i)); |
Ryosei | 25:d367d1e7a153 | 398 | } |
Ryosei | 25:d367d1e7a153 | 399 | Ult_left=UltraRead(0);//////////////////////////////////////////left sensor |
Ryosei | 25:d367d1e7a153 | 400 | Ult_right=UltraRead(1);//////////////////////////////////////////right sensor |
Ryosei | 25:d367d1e7a153 | 401 | |
Ryosei | 25:d367d1e7a153 | 402 | |
Ryosei | 25:d367d1e7a153 | 403 | // pc.printf("1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 7:%d 8:%d ULTRA0:%lf ULTRA1:%lf 1-2:%lf:\n\r",g[0],g[1],g[2],g[3],g[4],g[5],g[6],g[7],Ult_left,Ult_right,Ult_left-Ult_right); |
t_yamamoto | 0:669ef71cba68 | 404 | |
Ryosei | 25:d367d1e7a153 | 405 | //float a = ultrasonic[0].ReadDis(); |
Ryosei | 25:d367d1e7a153 | 406 | //pc.printf("%f\n\r",a); |
Ryosei | 25:d367d1e7a153 | 407 | |
Ryosei | 25:d367d1e7a153 | 408 | //int ppap = encoder[0].getPulses(); |
Ryosei | 25:d367d1e7a153 | 409 | //pc.printf("%d\n\r",ppap); |
Ryosei | 25:d367d1e7a153 | 410 | |
Ryosei | 25:d367d1e7a153 | 411 | |
Ryosei | 25:d367d1e7a153 | 412 | buzzer.period(1.0/800); |
Ryosei | 25:d367d1e7a153 | 413 | |
Ryosei | 25:d367d1e7a153 | 414 | #ifdef USE_MU |
Ryosei | 25:d367d1e7a153 | 415 | controller = CONTROLLER::Controller::GetData(); |
Ryosei | 25:d367d1e7a153 | 416 | #endif |
t_yamamoto | 0:669ef71cba68 | 417 | |
Ryosei | 25:d367d1e7a153 | 418 | #ifdef USE_ERRORCHECK |
Ryosei | 25:d367d1e7a153 | 419 | if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost) { |
Ryosei | 25:d367d1e7a153 | 420 | CONTROLLER::Controller::DataReset(); |
Ryosei | 25:d367d1e7a153 | 421 | AllActuatorReset(); |
Ryosei | 25:d367d1e7a153 | 422 | lock = true; |
Ryosei | 25:d367d1e7a153 | 423 | } else |
Ryosei | 25:d367d1e7a153 | 424 | #endif |
Ryosei | 25:d367d1e7a153 | 425 | { |
t_yamamoto | 0:669ef71cba68 | 426 | |
Ryosei | 25:d367d1e7a153 | 427 | #ifdef USE_SUBPROCESS |
Ryosei | 25:d367d1e7a153 | 428 | if(!lock) { |
Ryosei | 25:d367d1e7a153 | 429 | Process[current](); |
Ryosei | 25:d367d1e7a153 | 430 | } else |
Ryosei | 25:d367d1e7a153 | 431 | #endif |
Ryosei | 25:d367d1e7a153 | 432 | { |
Ryosei | 25:d367d1e7a153 | 433 | //ロック時の処理 |
Ryosei | 25:d367d1e7a153 | 434 | } |
Ryosei | 25:d367d1e7a153 | 435 | } |
Ryosei | 25:d367d1e7a153 | 436 | |
Ryosei | 25:d367d1e7a153 | 437 | |
Ryosei | 25:d367d1e7a153 | 438 | //Emergency! |
Ryosei | 25:d367d1e7a153 | 439 | /* |
Ryosei | 25:d367d1e7a153 | 440 | if(!EMG_0 && !EMG_1 && !EMGflag){ |
Ryosei | 25:d367d1e7a153 | 441 | buzzer = 0; |
Ryosei | 25:d367d1e7a153 | 442 | BuzzerTimer.attach(BuzzerTimer_func, 1); |
Ryosei | 25:d367d1e7a153 | 443 | EMGflag = true; |
Ryosei | 25:d367d1e7a153 | 444 | LED_DEBUG0 = 1; |
Ryosei | 25:d367d1e7a153 | 445 | } |
Ryosei | 25:d367d1e7a153 | 446 | if(EMG_0 && EMG_1 && EMGflag){ |
Ryosei | 25:d367d1e7a153 | 447 | buzzer = 1; |
Ryosei | 25:d367d1e7a153 | 448 | BuzzerTimer.detach(); |
Ryosei | 25:d367d1e7a153 | 449 | EMGflag = false; |
Ryosei | 25:d367d1e7a153 | 450 | } |
Ryosei | 25:d367d1e7a153 | 451 | */ |
Ryosei | 25:d367d1e7a153 | 452 | SystemProcessUpdate(); |
Ryosei | 25:d367d1e7a153 | 453 | } |
t_yamamoto | 0:669ef71cba68 | 454 | } |
t_yamamoto | 0:669ef71cba68 | 455 | |
kishibekairohan | 2:c015739085d3 | 456 | |
Ryosei | 25:d367d1e7a153 | 457 | |
kishibekairohan | 2:c015739085d3 | 458 | |
t_yamamoto | 0:669ef71cba68 | 459 | #pragma region PROCESS |
t_yamamoto | 0:669ef71cba68 | 460 | #ifdef USE_SUBPROCESS |
t_yamamoto | 0:669ef71cba68 | 461 | #if USE_PROCESS_NUM>0 |
Ryosei | 25:d367d1e7a153 | 462 | static void Process0() |
Ryosei | 25:d367d1e7a153 | 463 | { |
Ryosei | 25:d367d1e7a153 | 464 | AllActuatorReset(); |
t_yamamoto | 0:669ef71cba68 | 465 | } |
t_yamamoto | 0:669ef71cba68 | 466 | #endif |
Ryosei | 25:d367d1e7a153 | 467 | DigitalOut CLOTHESPIN(PB_9); |
t_yamamoto | 0:669ef71cba68 | 468 | #if USE_PROCESS_NUM>1 |
kishibekairohan | 16:3f2c2d89372b | 469 | static void Process1() |
t_yamamoto | 0:669ef71cba68 | 470 | { |
Ryosei | 25:d367d1e7a153 | 471 | LedOut(1); |
M_souta | 22:7d93f79a3686 | 472 | PIDflag = false; |
Ryosei | 25:d367d1e7a153 | 473 | if(controller->Button.ZR) { |
Ryosei | 25:d367d1e7a153 | 474 | CLOTHESPIN = 1; |
Ryosei | 25:d367d1e7a153 | 475 | } else { |
Ryosei | 25:d367d1e7a153 | 476 | CLOTHESPIN=0; |
Ryosei | 25:d367d1e7a153 | 477 | } |
Ryosei | 25:d367d1e7a153 | 478 | |
M_souta | 21:e3b58d675c1c | 479 | if(controller->Button.UP) { |
Ryosei | 25:d367d1e7a153 | 480 | motor[LIFT_LB].dir = FOR; |
Ryosei | 25:d367d1e7a153 | 481 | motor[LIFT_LB].pwm = 190; |
Ryosei | 25:d367d1e7a153 | 482 | motor[LIFT_RB].dir = BACK; |
Ryosei | 25:d367d1e7a153 | 483 | motor[LIFT_RB].pwm = 180; |
M_souta | 21:e3b58d675c1c | 484 | } else if(controller->Button.DOWN) { |
Ryosei | 25:d367d1e7a153 | 485 | motor[LIFT_LB].dir = BACK; |
Ryosei | 25:d367d1e7a153 | 486 | motor[LIFT_LB].pwm = 180; |
Ryosei | 25:d367d1e7a153 | 487 | motor[LIFT_RB].dir = FOR; |
Ryosei | 25:d367d1e7a153 | 488 | motor[LIFT_RB].pwm = 190; |
M_souta | 23:c853372cf626 | 489 | } else if(controller->Button.LEFT) { |
Ryosei | 25:d367d1e7a153 | 490 | motor[LIFT_LB].dir = FOR; |
Ryosei | 25:d367d1e7a153 | 491 | motor[LIFT_LB].pwm = 180; |
M_souta | 23:c853372cf626 | 492 | } else if(controller->Button.RIGHT) { |
Ryosei | 25:d367d1e7a153 | 493 | motor[LIFT_RB].dir = BACK; |
Ryosei | 25:d367d1e7a153 | 494 | motor[LIFT_RB].pwm = 180; |
M_souta | 21:e3b58d675c1c | 495 | } else { |
Ryosei | 25:d367d1e7a153 | 496 | motor[LIFT_LB].dir = FREE; |
Ryosei | 25:d367d1e7a153 | 497 | motor[LIFT_LB].pwm = 255; |
Ryosei | 25:d367d1e7a153 | 498 | motor[LIFT_RB].dir = BACK; |
Ryosei | 25:d367d1e7a153 | 499 | motor[LIFT_RB].pwm = 10; |
M_souta | 21:e3b58d675c1c | 500 | } |
M_souta | 21:e3b58d675c1c | 501 | |
Ryosei | 25:d367d1e7a153 | 502 | |
Ryosei | 25:d367d1e7a153 | 503 | if(controller->Button.X) { |
Ryosei | 25:d367d1e7a153 | 504 | motor[LIFT_U].dir = FOR; |
Ryosei | 25:d367d1e7a153 | 505 | motor[LIFT_U].pwm = 180; |
Ryosei | 25:d367d1e7a153 | 506 | } else if(controller->Button.Y) { |
Ryosei | 25:d367d1e7a153 | 507 | motor[LIFT_U].dir = BACK; |
Ryosei | 25:d367d1e7a153 | 508 | motor[LIFT_U].pwm = 180; |
Ryosei | 25:d367d1e7a153 | 509 | } else { |
Ryosei | 25:d367d1e7a153 | 510 | motor[LIFT_U].dir = BRAKE; |
Ryosei | 25:d367d1e7a153 | 511 | motor[LIFT_U].pwm = 180; |
Ryosei | 25:d367d1e7a153 | 512 | } |
Ryosei | 25:d367d1e7a153 | 513 | |
Ryosei | 25:d367d1e7a153 | 514 | if(!(controller->AnalogL.Y == 7) || !(controller->AnalogL.X == 7)) { |
Ryosei | 25:d367d1e7a153 | 515 | motor[TIRE_FR].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] ); |
Ryosei | 25:d367d1e7a153 | 516 | motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X] ); |
Ryosei | 25:d367d1e7a153 | 517 | motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] ); |
Ryosei | 25:d367d1e7a153 | 518 | motor[TIRE_BL].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y] ); |
Ryosei | 25:d367d1e7a153 | 519 | |
Ryosei | 25:d367d1e7a153 | 520 | motor[TIRE_FR].pwm = SetPWM(omni[controller->AnalogL.Y][14-controller->AnalogL.X] * 0.2) ; |
Ryosei | 25:d367d1e7a153 | 521 | motor[TIRE_FL].pwm = SetPWM(omni[controller->AnalogL.Y][controller->AnalogL.X] * 0.2) ; |
Ryosei | 25:d367d1e7a153 | 522 | motor[TIRE_BR].pwm = SetPWM(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] * 0.2) ; |
Ryosei | 25:d367d1e7a153 | 523 | motor[TIRE_BL].pwm = SetPWM(omni[controller->AnalogL.X][14-controller->AnalogL.Y] * 0.2) ; |
Ryosei | 25:d367d1e7a153 | 524 | } else { |
Ryosei | 25:d367d1e7a153 | 525 | motor[TIRE_BL].dir = SetStatus(curve[controller->AnalogR.X]); |
Ryosei | 25:d367d1e7a153 | 526 | motor[TIRE_FL].dir = SetStatus(curve[controller->AnalogR.X]); |
Ryosei | 25:d367d1e7a153 | 527 | motor[TIRE_BR].dir = SetStatus(curve[controller->AnalogR.X]); |
Ryosei | 25:d367d1e7a153 | 528 | motor[TIRE_FR].dir = SetStatus(curve[controller->AnalogR.X]); |
Ryosei | 25:d367d1e7a153 | 529 | |
Ryosei | 25:d367d1e7a153 | 530 | motor[TIRE_FR].pwm = SetPWM(curve[controller->AnalogR.X]); |
Ryosei | 25:d367d1e7a153 | 531 | motor[TIRE_FL].pwm = SetPWM(curve[controller->AnalogR.X]); |
Ryosei | 25:d367d1e7a153 | 532 | motor[TIRE_BR].pwm = SetPWM(curve[controller->AnalogR.X]); |
Ryosei | 25:d367d1e7a153 | 533 | motor[TIRE_BL].pwm = SetPWM(curve[controller->AnalogR.X]); |
Ryosei | 25:d367d1e7a153 | 534 | } |
Ryosei | 25:d367d1e7a153 | 535 | |
t_yamamoto | 0:669ef71cba68 | 536 | } |
t_yamamoto | 0:669ef71cba68 | 537 | #endif |
t_yamamoto | 0:669ef71cba68 | 538 | |
t_yamamoto | 0:669ef71cba68 | 539 | #if USE_PROCESS_NUM>2 |
kishibekairohan | 16:3f2c2d89372b | 540 | static void Process2() |
Ryosei | 25:d367d1e7a153 | 541 | { |
Ryosei | 25:d367d1e7a153 | 542 | |
Ryosei | 25:d367d1e7a153 | 543 | static int phase=0;//system phase |
Ryosei | 25:d367d1e7a153 | 544 | static int StartFlag=0;//if start switch is pressed,the valiable sets "1". |
Ryosei | 25:d367d1e7a153 | 545 | pc.printf("phase:%d,air:%d\r\n",phase,CLOTHESPIN); |
Ryosei | 25:d367d1e7a153 | 546 | Timer FlagTimer;//調整中に超音波センサーが変な動きをした時のためのタイマー |
Ryosei | 25:d367d1e7a153 | 547 | Timer AirTimer;//エアーのタイマー |
Ryosei | 25:d367d1e7a153 | 548 | int RedOrBlue=LimitSw::IsPressed(RED_OR_BLUE); |
Ryosei | 25:d367d1e7a153 | 549 | // solenoid.solenoid0=1; |
Ryosei | 25:d367d1e7a153 | 550 | if(LimitSw::IsPressed(STARTSW)) { |
Ryosei | 25:d367d1e7a153 | 551 | CLOTHESPIN=0; |
Ryosei | 25:d367d1e7a153 | 552 | StartFlag=1; |
Ryosei | 25:d367d1e7a153 | 553 | } |
Ryosei | 25:d367d1e7a153 | 554 | if(controller->Button.ZL) { |
Ryosei | 25:d367d1e7a153 | 555 | CLOTHESPIN=0; |
Ryosei | 25:d367d1e7a153 | 556 | motor[TIRE_FL].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 557 | motor[TIRE_FR].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 558 | motor[TIRE_BL].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 559 | motor[TIRE_BR].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 560 | motor[TIRE_FL].pwm=15; |
Ryosei | 25:d367d1e7a153 | 561 | motor[TIRE_FR].pwm=15; |
Ryosei | 25:d367d1e7a153 | 562 | motor[TIRE_BL].pwm=15; |
Ryosei | 25:d367d1e7a153 | 563 | motor[TIRE_BR].pwm=15; |
Ryosei | 25:d367d1e7a153 | 564 | } else { |
Ryosei | 25:d367d1e7a153 | 565 | CLOTHESPIN=0; |
Ryosei | 25:d367d1e7a153 | 566 | } |
Ryosei | 25:d367d1e7a153 | 567 | if((controller->Button.A)||(StartFlag==1)) { |
Ryosei | 25:d367d1e7a153 | 568 | if(phase==0) { |
Ryosei | 25:d367d1e7a153 | 569 | //位置調整 |
Ryosei | 25:d367d1e7a153 | 570 | //(P制御) |
Ryosei | 25:d367d1e7a153 | 571 | CLOTHESPIN=0; |
Ryosei | 25:d367d1e7a153 | 572 | LedOut(4); |
Ryosei | 25:d367d1e7a153 | 573 | if((Ult_left>0)&&(Ult_right>0)) {//データを受け取っているとき |
Ryosei | 25:d367d1e7a153 | 574 | if((Ult_left<16)&&(Ult_left>14)&&(Ult_right<16)&&(Ult_right>14)) { //合った場合 |
Ryosei | 25:d367d1e7a153 | 575 | motor[TIRE_FL].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 576 | motor[TIRE_FR].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 577 | motor[TIRE_BL].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 578 | motor[TIRE_BR].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 579 | motor[TIRE_FL].pwm=100; |
Ryosei | 25:d367d1e7a153 | 580 | motor[TIRE_FR].pwm=100; |
Ryosei | 25:d367d1e7a153 | 581 | motor[TIRE_BL].pwm=100; |
Ryosei | 25:d367d1e7a153 | 582 | motor[TIRE_BR].pwm=100; |
Ryosei | 25:d367d1e7a153 | 583 | phase=1;//system phase increasing |
Ryosei | 25:d367d1e7a153 | 584 | } else if((Ult_left<16)&&(Ult_left>14)) { //Ult_leftのみあった場合 |
Ryosei | 25:d367d1e7a153 | 585 | if(Ult_right>16) { //Ult_rightが遠い場合 |
Ryosei | 25:d367d1e7a153 | 586 | motor[TIRE_FL].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 587 | motor[TIRE_FR].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 588 | motor[TIRE_BL].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 589 | motor[TIRE_BR].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 590 | motor[TIRE_FL].pwm=0; |
Ryosei | 25:d367d1e7a153 | 591 | motor[TIRE_FR].pwm=15; |
Ryosei | 25:d367d1e7a153 | 592 | motor[TIRE_BL].pwm=0; |
Ryosei | 25:d367d1e7a153 | 593 | motor[TIRE_BR].pwm=15; |
Ryosei | 25:d367d1e7a153 | 594 | } else if(Ult_right<14) { //Ult_rightが近い場合 |
Ryosei | 25:d367d1e7a153 | 595 | motor[TIRE_FL].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 596 | motor[TIRE_FR].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 597 | motor[TIRE_BL].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 598 | motor[TIRE_BR].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 599 | motor[TIRE_FL].pwm=0; |
Ryosei | 25:d367d1e7a153 | 600 | motor[TIRE_FR].pwm=15; |
Ryosei | 25:d367d1e7a153 | 601 | motor[TIRE_BL].pwm=0; |
Ryosei | 25:d367d1e7a153 | 602 | motor[TIRE_BR].pwm=15; |
Ryosei | 25:d367d1e7a153 | 603 | } |
Ryosei | 25:d367d1e7a153 | 604 | } else if((Ult_right<16)&&(Ult_right>14)) { //Ult_rightのみあった場合 |
Ryosei | 25:d367d1e7a153 | 605 | if(Ult_left>16) { //Ult_leftが遠い場合 |
Ryosei | 25:d367d1e7a153 | 606 | motor[TIRE_FL].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 607 | motor[TIRE_FR].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 608 | motor[TIRE_BL].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 609 | motor[TIRE_BR].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 610 | motor[TIRE_FL].pwm=15; |
Ryosei | 25:d367d1e7a153 | 611 | motor[TIRE_FR].pwm=0; |
Ryosei | 25:d367d1e7a153 | 612 | motor[TIRE_BL].pwm=15; |
Ryosei | 25:d367d1e7a153 | 613 | motor[TIRE_BR].pwm=0; |
Ryosei | 25:d367d1e7a153 | 614 | } else if(Ult_left<14) { //Ult_leftが近い場合 |
Ryosei | 25:d367d1e7a153 | 615 | motor[TIRE_FL].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 616 | motor[TIRE_FR].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 617 | motor[TIRE_BL].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 618 | motor[TIRE_BR].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 619 | motor[TIRE_FL].pwm=15; |
Ryosei | 25:d367d1e7a153 | 620 | motor[TIRE_FR].pwm=0; |
Ryosei | 25:d367d1e7a153 | 621 | motor[TIRE_BL].pwm=15; |
Ryosei | 25:d367d1e7a153 | 622 | motor[TIRE_BR].pwm=0; |
Ryosei | 25:d367d1e7a153 | 623 | } |
Ryosei | 25:d367d1e7a153 | 624 | } else { //どっちもあってない場合 |
Ryosei | 25:d367d1e7a153 | 625 | if( (Ult_left+Ult_right)<=25||(Ult_left+Ult_right)>=35) {//離れすぎているor近すぎるとき |
Ryosei | 25:d367d1e7a153 | 626 | if((Ult_left-Ult_right)>10||((Ult_left-Ult_right)<-10) ) { //傾きが大きいとき |
Ryosei | 25:d367d1e7a153 | 627 | if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき |
Ryosei | 25:d367d1e7a153 | 628 | motor[TIRE_FL].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 629 | motor[TIRE_FR].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 630 | motor[TIRE_BL].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 631 | motor[TIRE_BR].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 632 | motor[TIRE_FL].pwm=15; |
Ryosei | 25:d367d1e7a153 | 633 | motor[TIRE_FR].pwm=15; |
Ryosei | 25:d367d1e7a153 | 634 | motor[TIRE_BL].pwm=15; |
Ryosei | 25:d367d1e7a153 | 635 | motor[TIRE_BR].pwm=15; |
Ryosei | 25:d367d1e7a153 | 636 | } else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき |
Ryosei | 25:d367d1e7a153 | 637 | motor[TIRE_FL].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 638 | motor[TIRE_FR].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 639 | motor[TIRE_BL].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 640 | motor[TIRE_BR].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 641 | motor[TIRE_FL].pwm=15; |
Ryosei | 25:d367d1e7a153 | 642 | motor[TIRE_FR].pwm=15; |
Ryosei | 25:d367d1e7a153 | 643 | motor[TIRE_BL].pwm=15; |
Ryosei | 25:d367d1e7a153 | 644 | motor[TIRE_BR].pwm=15; |
Ryosei | 25:d367d1e7a153 | 645 | } |
Ryosei | 25:d367d1e7a153 | 646 | } else { //傾きが大きくなくて離れているとき |
Ryosei | 25:d367d1e7a153 | 647 | if((Ult_right+Ult_left)<=25) { //近すぎるとき |
Ryosei | 25:d367d1e7a153 | 648 | motor[TIRE_FL].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 649 | motor[TIRE_FR].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 650 | motor[TIRE_BL].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 651 | motor[TIRE_BR].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 652 | motor[TIRE_FL].pwm=20; |
Ryosei | 25:d367d1e7a153 | 653 | motor[TIRE_FR].pwm=20; |
Ryosei | 25:d367d1e7a153 | 654 | motor[TIRE_BL].pwm=20; |
Ryosei | 25:d367d1e7a153 | 655 | motor[TIRE_BR].pwm=20; |
Ryosei | 25:d367d1e7a153 | 656 | } else if((Ult_right+Ult_left)>=35) { //離れているとき |
Ryosei | 25:d367d1e7a153 | 657 | motor[TIRE_FL].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 658 | motor[TIRE_FR].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 659 | motor[TIRE_BL].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 660 | motor[TIRE_BR].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 661 | motor[TIRE_FL].pwm=20; |
Ryosei | 25:d367d1e7a153 | 662 | motor[TIRE_FR].pwm=20; |
Ryosei | 25:d367d1e7a153 | 663 | motor[TIRE_BL].pwm=20; |
Ryosei | 25:d367d1e7a153 | 664 | motor[TIRE_BR].pwm=20; |
Ryosei | 25:d367d1e7a153 | 665 | } |
Ryosei | 25:d367d1e7a153 | 666 | } |
Ryosei | 25:d367d1e7a153 | 667 | |
Ryosei | 25:d367d1e7a153 | 668 | } else { //さほど離れてはいないが傾きが大きいとき |
Ryosei | 25:d367d1e7a153 | 669 | if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき |
Ryosei | 25:d367d1e7a153 | 670 | motor[TIRE_FL].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 671 | motor[TIRE_FR].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 672 | motor[TIRE_BL].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 673 | motor[TIRE_BR].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 674 | motor[TIRE_FL].pwm=15; |
Ryosei | 25:d367d1e7a153 | 675 | motor[TIRE_FR].pwm=15; |
Ryosei | 25:d367d1e7a153 | 676 | motor[TIRE_BL].pwm=15; |
Ryosei | 25:d367d1e7a153 | 677 | motor[TIRE_BR].pwm=15; |
Ryosei | 25:d367d1e7a153 | 678 | } else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき |
Ryosei | 25:d367d1e7a153 | 679 | motor[TIRE_FL].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 680 | motor[TIRE_FR].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 681 | motor[TIRE_BL].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 682 | motor[TIRE_BR].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 683 | motor[TIRE_FL].pwm=15; |
Ryosei | 25:d367d1e7a153 | 684 | motor[TIRE_FR].pwm=15; |
Ryosei | 25:d367d1e7a153 | 685 | motor[TIRE_BL].pwm=15; |
Ryosei | 25:d367d1e7a153 | 686 | motor[TIRE_BR].pwm=15; |
Ryosei | 25:d367d1e7a153 | 687 | } |
Ryosei | 25:d367d1e7a153 | 688 | } |
Ryosei | 25:d367d1e7a153 | 689 | } |
Ryosei | 25:d367d1e7a153 | 690 | |
Ryosei | 25:d367d1e7a153 | 691 | } else {//データを受け取ってないとき |
Ryosei | 25:d367d1e7a153 | 692 | motor[TIRE_FL].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 693 | motor[TIRE_FR].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 694 | motor[TIRE_BL].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 695 | motor[TIRE_BR].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 696 | motor[TIRE_FL].pwm=100; |
Ryosei | 25:d367d1e7a153 | 697 | motor[TIRE_FR].pwm=100; |
Ryosei | 25:d367d1e7a153 | 698 | motor[TIRE_BL].pwm=100; |
Ryosei | 25:d367d1e7a153 | 699 | motor[TIRE_BR].pwm=100; |
Ryosei | 25:d367d1e7a153 | 700 | } |
Ryosei | 25:d367d1e7a153 | 701 | } else if(phase==1) { |
Ryosei | 25:d367d1e7a153 | 702 | phase=2; |
Ryosei | 25:d367d1e7a153 | 703 | |
Ryosei | 25:d367d1e7a153 | 704 | |
Ryosei | 25:d367d1e7a153 | 705 | //待機&phase1に戻る |
Ryosei | 25:d367d1e7a153 | 706 | |
Ryosei | 25:d367d1e7a153 | 707 | } else if(phase==2) { |
Ryosei | 25:d367d1e7a153 | 708 | CLOTHESPIN=SOLENOID_OFF; |
Ryosei | 25:d367d1e7a153 | 709 | LedOut(1); |
Ryosei | 25:d367d1e7a153 | 710 | //リミットスイッチに当てる |
Ryosei | 25:d367d1e7a153 | 711 | static int count2=0; |
Ryosei | 25:d367d1e7a153 | 712 | if(count2==0) { |
Ryosei | 25:d367d1e7a153 | 713 | if((Ult_left>0)&&(Ult_right>0)) {//データを受け取っているとき |
Ryosei | 25:d367d1e7a153 | 714 | if( (Ult_left+Ult_right)<=25||(Ult_left+Ult_right)>=35) { |
Ryosei | 25:d367d1e7a153 | 715 | if(((Ult_left-Ult_right)>=5)||((Ult_left-Ult_right)<=-5)) { |
Ryosei | 25:d367d1e7a153 | 716 | if(Ult_left-Ult_right<=0) { |
Ryosei | 25:d367d1e7a153 | 717 | motor[TIRE_FL].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 718 | motor[TIRE_FR].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 719 | motor[TIRE_BL].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 720 | motor[TIRE_BR].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 721 | |
Ryosei | 25:d367d1e7a153 | 722 | motor[TIRE_FL].pwm=25+(-1*(Ult_left-Ult_right)); |
Ryosei | 25:d367d1e7a153 | 723 | motor[TIRE_FR].pwm=25+(-1*(Ult_left-Ult_right)); |
Ryosei | 25:d367d1e7a153 | 724 | |
Ryosei | 25:d367d1e7a153 | 725 | motor[TIRE_BL].pwm=25; |
Ryosei | 25:d367d1e7a153 | 726 | motor[TIRE_BR].pwm=25; |
Ryosei | 25:d367d1e7a153 | 727 | } else if(Ult_left-Ult_right>0) { |
Ryosei | 25:d367d1e7a153 | 728 | motor[TIRE_FL].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 729 | motor[TIRE_FR].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 730 | motor[TIRE_BL].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 731 | motor[TIRE_BR].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 732 | motor[TIRE_BL].pwm=25+(1*(Ult_left-Ult_right)); |
Ryosei | 25:d367d1e7a153 | 733 | motor[TIRE_BR].pwm=25+(1*(Ult_left-Ult_right)); |
Ryosei | 25:d367d1e7a153 | 734 | motor[TIRE_FL].pwm=25; |
Ryosei | 25:d367d1e7a153 | 735 | motor[TIRE_FR].pwm=25; |
Ryosei | 25:d367d1e7a153 | 736 | } |
Ryosei | 25:d367d1e7a153 | 737 | } else if((Ult_left+Ult_right)<=25) { |
Ryosei | 25:d367d1e7a153 | 738 | motor[TIRE_FL].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 739 | motor[TIRE_FR].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 740 | motor[TIRE_BL].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 741 | motor[TIRE_BR].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 742 | motor[TIRE_FL].pwm=25; |
Ryosei | 25:d367d1e7a153 | 743 | motor[TIRE_FR].pwm=0; |
Ryosei | 25:d367d1e7a153 | 744 | motor[TIRE_BL].pwm=0; |
Ryosei | 25:d367d1e7a153 | 745 | motor[TIRE_BR].pwm=25; |
Ryosei | 25:d367d1e7a153 | 746 | } else if((Ult_left+Ult_right)>=35) { |
Ryosei | 25:d367d1e7a153 | 747 | motor[TIRE_FL].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 748 | motor[TIRE_FR].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 749 | motor[TIRE_BL].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 750 | motor[TIRE_BR].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 751 | motor[TIRE_FL].pwm=0; |
Ryosei | 25:d367d1e7a153 | 752 | motor[TIRE_FR].pwm=25; |
Ryosei | 25:d367d1e7a153 | 753 | motor[TIRE_BL].pwm=25; |
Ryosei | 25:d367d1e7a153 | 754 | motor[TIRE_BR].pwm=0; |
Ryosei | 25:d367d1e7a153 | 755 | } |
Ryosei | 25:d367d1e7a153 | 756 | } else { |
Ryosei | 25:d367d1e7a153 | 757 | if(Ult_left-Ult_right<=0) { |
Ryosei | 25:d367d1e7a153 | 758 | motor[TIRE_FL].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 759 | motor[TIRE_FR].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 760 | motor[TIRE_BL].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 761 | motor[TIRE_BR].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 762 | |
Ryosei | 25:d367d1e7a153 | 763 | motor[TIRE_FL].pwm=25+(-1*(Ult_left-Ult_right)); |
Ryosei | 25:d367d1e7a153 | 764 | motor[TIRE_FR].pwm=25+(-1*(Ult_left-Ult_right)); |
Ryosei | 25:d367d1e7a153 | 765 | |
Ryosei | 25:d367d1e7a153 | 766 | motor[TIRE_BL].pwm=25; |
Ryosei | 25:d367d1e7a153 | 767 | motor[TIRE_BR].pwm=25; |
Ryosei | 25:d367d1e7a153 | 768 | } else if(Ult_left-Ult_right>0) { |
Ryosei | 25:d367d1e7a153 | 769 | motor[TIRE_FL].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 770 | motor[TIRE_FR].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 771 | motor[TIRE_BL].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 772 | motor[TIRE_BR].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 773 | motor[TIRE_BL].pwm=25+(1*(Ult_left-Ult_right)); |
Ryosei | 25:d367d1e7a153 | 774 | motor[TIRE_BR].pwm=25+(1*(Ult_left-Ult_right)); |
Ryosei | 25:d367d1e7a153 | 775 | motor[TIRE_FL].pwm=25; |
Ryosei | 25:d367d1e7a153 | 776 | motor[TIRE_FR].pwm=25; |
Ryosei | 25:d367d1e7a153 | 777 | } |
Ryosei | 25:d367d1e7a153 | 778 | } |
Ryosei | 25:d367d1e7a153 | 779 | |
Ryosei | 25:d367d1e7a153 | 780 | } else { |
Ryosei | 25:d367d1e7a153 | 781 | motor[TIRE_FL].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 782 | motor[TIRE_FR].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 783 | motor[TIRE_BL].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 784 | motor[TIRE_BR].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 785 | motor[TIRE_FL].pwm=100; |
Ryosei | 25:d367d1e7a153 | 786 | motor[TIRE_FR].pwm=100; |
Ryosei | 25:d367d1e7a153 | 787 | motor[TIRE_BL].pwm=100; |
Ryosei | 25:d367d1e7a153 | 788 | motor[TIRE_BR].pwm=100; |
Ryosei | 25:d367d1e7a153 | 789 | |
Ryosei | 25:d367d1e7a153 | 790 | } |
Ryosei | 25:d367d1e7a153 | 791 | if(LimitSw::IsPressed(LEFTlim)) { |
Ryosei | 25:d367d1e7a153 | 792 | count2=1; |
Ryosei | 25:d367d1e7a153 | 793 | } |
Ryosei | 25:d367d1e7a153 | 794 | |
Ryosei | 25:d367d1e7a153 | 795 | } else if(count2==1) { |
Ryosei | 25:d367d1e7a153 | 796 | motor[TIRE_FL].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 797 | motor[TIRE_FR].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 798 | motor[TIRE_BL].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 799 | motor[TIRE_BR].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 800 | motor[TIRE_FL].pwm=255; |
Ryosei | 25:d367d1e7a153 | 801 | motor[TIRE_FR].pwm=255; |
Ryosei | 25:d367d1e7a153 | 802 | motor[TIRE_BL].pwm=255; |
Ryosei | 25:d367d1e7a153 | 803 | motor[TIRE_BR].pwm=255; |
Ryosei | 25:d367d1e7a153 | 804 | phase=10; |
Ryosei | 25:d367d1e7a153 | 805 | } |
Ryosei | 25:d367d1e7a153 | 806 | } else if(phase==3) { |
Ryosei | 25:d367d1e7a153 | 807 | //洗濯ばさみを止める&戻す |
Ryosei | 25:d367d1e7a153 | 808 | static int count3=0; |
Ryosei | 25:d367d1e7a153 | 809 | static int loop=0; |
Ryosei | 25:d367d1e7a153 | 810 | pc.printf("%d\r\n",loop); |
Ryosei | 25:d367d1e7a153 | 811 | CLOTHESPIN=1; |
Ryosei | 25:d367d1e7a153 | 812 | if(count3==0) { |
Ryosei | 25:d367d1e7a153 | 813 | loop++; |
Ryosei | 25:d367d1e7a153 | 814 | if(loop==60) { |
Ryosei | 25:d367d1e7a153 | 815 | count3=1; |
Ryosei | 25:d367d1e7a153 | 816 | } |
Ryosei | 25:d367d1e7a153 | 817 | } else if(count3==1) { |
Ryosei | 25:d367d1e7a153 | 818 | CLOTHESPIN=0; |
Ryosei | 25:d367d1e7a153 | 819 | phase=4; |
Ryosei | 25:d367d1e7a153 | 820 | |
Ryosei | 25:d367d1e7a153 | 821 | } |
Ryosei | 25:d367d1e7a153 | 822 | } else if(phase==4) { |
Ryosei | 25:d367d1e7a153 | 823 | //シーツをかける |
Ryosei | 25:d367d1e7a153 | 824 | LedOut(3); |
Ryosei | 25:d367d1e7a153 | 825 | CLOTHESPIN=0; |
Ryosei | 25:d367d1e7a153 | 826 | if(!(LimitSw::IsPressed(RIGHTlim))) { |
Ryosei | 25:d367d1e7a153 | 827 | |
Ryosei | 25:d367d1e7a153 | 828 | //超音波 |
Ryosei | 25:d367d1e7a153 | 829 | if(Ult_right>0) { |
Ryosei | 25:d367d1e7a153 | 830 | if(Ult_right<15) { |
Ryosei | 25:d367d1e7a153 | 831 | motor[TIRE_FL].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 832 | motor[TIRE_FR].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 833 | motor[TIRE_BL].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 834 | motor[TIRE_BR].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 835 | motor[TIRE_FL].pwm=25; |
Ryosei | 25:d367d1e7a153 | 836 | motor[TIRE_FR].pwm=25+(15-Ult_right); |
Ryosei | 25:d367d1e7a153 | 837 | motor[TIRE_BL].pwm=25+(15-Ult_right); |
Ryosei | 25:d367d1e7a153 | 838 | motor[TIRE_BR].pwm=25; |
Ryosei | 25:d367d1e7a153 | 839 | } else if(Ult_right>=15) { |
Ryosei | 25:d367d1e7a153 | 840 | motor[TIRE_FL].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 841 | motor[TIRE_FR].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 842 | motor[TIRE_BL].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 843 | motor[TIRE_BR].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 844 | motor[TIRE_FL].pwm=25+(Ult_right-15); |
Ryosei | 25:d367d1e7a153 | 845 | motor[TIRE_FR].pwm=25; |
Ryosei | 25:d367d1e7a153 | 846 | motor[TIRE_BL].pwm=25; |
Ryosei | 25:d367d1e7a153 | 847 | motor[TIRE_BR].pwm=25+(Ult_right-15); |
Ryosei | 25:d367d1e7a153 | 848 | } |
Ryosei | 25:d367d1e7a153 | 849 | } else { |
Ryosei | 25:d367d1e7a153 | 850 | motor[TIRE_FL].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 851 | motor[TIRE_FR].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 852 | motor[TIRE_BL].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 853 | motor[TIRE_BR].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 854 | motor[TIRE_FL].pwm=25; |
Ryosei | 25:d367d1e7a153 | 855 | motor[TIRE_FR].pwm=25; |
Ryosei | 25:d367d1e7a153 | 856 | motor[TIRE_BL].pwm=25; |
Ryosei | 25:d367d1e7a153 | 857 | motor[TIRE_BR].pwm=25; |
Ryosei | 25:d367d1e7a153 | 858 | } |
Ryosei | 25:d367d1e7a153 | 859 | |
Ryosei | 25:d367d1e7a153 | 860 | /* |
Ryosei | 25:d367d1e7a153 | 861 | if((Ult_left>0)&&(Ult_right>0)) {//データを受け取っているとき |
Ryosei | 25:d367d1e7a153 | 862 | if( (Ult_left+Ult_right)<=25||(Ult_left+Ult_right)>=35) { |
Ryosei | 25:d367d1e7a153 | 863 | if(((Ult_left-Ult_right)>=5)||((Ult_left-Ult_right)<=-5)) { |
Ryosei | 25:d367d1e7a153 | 864 | if(Ult_left-Ult_right<=0) { |
Ryosei | 25:d367d1e7a153 | 865 | motor[TIRE_FL].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 866 | motor[TIRE_FR].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 867 | motor[TIRE_BL].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 868 | motor[TIRE_BR].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 869 | |
Ryosei | 25:d367d1e7a153 | 870 | motor[TIRE_BL].pwm=25+(-1*(Ult_left-Ult_right)); |
Ryosei | 25:d367d1e7a153 | 871 | motor[TIRE_BR].pwm=25+(-1*(Ult_left-Ult_right)); |
Ryosei | 25:d367d1e7a153 | 872 | |
Ryosei | 25:d367d1e7a153 | 873 | motor[TIRE_FL].pwm=25; |
Ryosei | 25:d367d1e7a153 | 874 | motor[TIRE_FR].pwm=25; |
Ryosei | 25:d367d1e7a153 | 875 | } else if(Ult_left-Ult_right>0) { |
Ryosei | 25:d367d1e7a153 | 876 | motor[TIRE_FL].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 877 | motor[TIRE_FR].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 878 | motor[TIRE_BL].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 879 | motor[TIRE_BR].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 880 | motor[TIRE_FL].pwm=25+(1*(Ult_left-Ult_right)); |
Ryosei | 25:d367d1e7a153 | 881 | motor[TIRE_FR].pwm=25+(1*(Ult_left-Ult_right)); |
Ryosei | 25:d367d1e7a153 | 882 | motor[TIRE_BL].pwm=25; |
Ryosei | 25:d367d1e7a153 | 883 | motor[TIRE_BR].pwm=25; |
Ryosei | 25:d367d1e7a153 | 884 | } |
Ryosei | 25:d367d1e7a153 | 885 | } else if((Ult_left+Ult_right)<=25) { |
Ryosei | 25:d367d1e7a153 | 886 | motor[TIRE_FL].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 887 | motor[TIRE_FR].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 888 | motor[TIRE_BL].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 889 | motor[TIRE_BR].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 890 | motor[TIRE_FL].pwm=0; |
Ryosei | 25:d367d1e7a153 | 891 | motor[TIRE_FR].pwm=25; |
Ryosei | 25:d367d1e7a153 | 892 | motor[TIRE_BL].pwm=25; |
Ryosei | 25:d367d1e7a153 | 893 | motor[TIRE_BR].pwm=0; |
Ryosei | 25:d367d1e7a153 | 894 | } else if((Ult_left+Ult_right)>=35) { |
Ryosei | 25:d367d1e7a153 | 895 | motor[TIRE_FL].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 896 | motor[TIRE_FR].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 897 | motor[TIRE_BL].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 898 | motor[TIRE_BR].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 899 | motor[TIRE_FL].pwm=25; |
Ryosei | 25:d367d1e7a153 | 900 | motor[TIRE_FR].pwm=0; |
Ryosei | 25:d367d1e7a153 | 901 | motor[TIRE_BL].pwm=0; |
Ryosei | 25:d367d1e7a153 | 902 | motor[TIRE_BR].pwm=25; |
Ryosei | 25:d367d1e7a153 | 903 | } |
Ryosei | 25:d367d1e7a153 | 904 | } else { |
Ryosei | 25:d367d1e7a153 | 905 | if(Ult_left-Ult_right<=0) { |
Ryosei | 25:d367d1e7a153 | 906 | motor[TIRE_FL].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 907 | motor[TIRE_FR].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 908 | motor[TIRE_BL].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 909 | motor[TIRE_BR].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 910 | |
Ryosei | 25:d367d1e7a153 | 911 | motor[TIRE_BL].pwm=25+(-1*(Ult_left-Ult_right)); |
Ryosei | 25:d367d1e7a153 | 912 | motor[TIRE_BR].pwm=25+(-1*(Ult_left-Ult_right)); |
Ryosei | 25:d367d1e7a153 | 913 | |
Ryosei | 25:d367d1e7a153 | 914 | motor[TIRE_FL].pwm=25; |
Ryosei | 25:d367d1e7a153 | 915 | motor[TIRE_FR].pwm=25; |
Ryosei | 25:d367d1e7a153 | 916 | |
Ryosei | 25:d367d1e7a153 | 917 | } else if(Ult_left-Ult_right>0) { |
Ryosei | 25:d367d1e7a153 | 918 | motor[TIRE_FL].pwm=25+(1*(Ult_left-Ult_right)); |
Ryosei | 25:d367d1e7a153 | 919 | motor[TIRE_FR].pwm=25+(1*(Ult_left-Ult_right)); |
Ryosei | 25:d367d1e7a153 | 920 | motor[TIRE_BL].pwm=25; |
Ryosei | 25:d367d1e7a153 | 921 | motor[TIRE_BR].pwm=25; |
Ryosei | 25:d367d1e7a153 | 922 | } |
Ryosei | 25:d367d1e7a153 | 923 | } |
Ryosei | 25:d367d1e7a153 | 924 | } else { |
Ryosei | 25:d367d1e7a153 | 925 | motor[TIRE_FL].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 926 | motor[TIRE_FR].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 927 | motor[TIRE_BL].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 928 | motor[TIRE_BR].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 929 | motor[TIRE_FL].pwm=100; |
Ryosei | 25:d367d1e7a153 | 930 | motor[TIRE_FR].pwm=100; |
Ryosei | 25:d367d1e7a153 | 931 | motor[TIRE_BL].pwm=100; |
Ryosei | 25:d367d1e7a153 | 932 | motor[TIRE_BR].pwm=100; |
Ryosei | 25:d367d1e7a153 | 933 | |
Ryosei | 25:d367d1e7a153 | 934 | } |
Ryosei | 25:d367d1e7a153 | 935 | */ |
Ryosei | 25:d367d1e7a153 | 936 | } else { |
Ryosei | 25:d367d1e7a153 | 937 | motor[TIRE_FL].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 938 | motor[TIRE_FR].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 939 | motor[TIRE_BL].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 940 | motor[TIRE_BR].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 941 | motor[TIRE_FL].pwm=100; |
Ryosei | 25:d367d1e7a153 | 942 | motor[TIRE_FR].pwm=100; |
Ryosei | 25:d367d1e7a153 | 943 | motor[TIRE_BL].pwm=100; |
Ryosei | 25:d367d1e7a153 | 944 | motor[TIRE_BR].pwm=100; |
Ryosei | 25:d367d1e7a153 | 945 | phase=5; |
Ryosei | 25:d367d1e7a153 | 946 | } |
Ryosei | 25:d367d1e7a153 | 947 | } else if(phase==5) { |
Ryosei | 25:d367d1e7a153 | 948 | motor[TIRE_FL].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 949 | motor[TIRE_FR].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 950 | motor[TIRE_BL].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 951 | motor[TIRE_BR].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 952 | motor[TIRE_FL].pwm=30; |
Ryosei | 25:d367d1e7a153 | 953 | motor[TIRE_FR].pwm=100; |
Ryosei | 25:d367d1e7a153 | 954 | motor[TIRE_BL].pwm=100; |
Ryosei | 25:d367d1e7a153 | 955 | motor[TIRE_BR].pwm=30; |
Ryosei | 25:d367d1e7a153 | 956 | //斜め後ろに下がる |
Ryosei | 25:d367d1e7a153 | 957 | } else if(phase==10) { |
Ryosei | 25:d367d1e7a153 | 958 | if((Ult_left>0)&&(Ult_right>0)) {//データを受け取っているとき |
Ryosei | 25:d367d1e7a153 | 959 | if((Ult_left<16)&&(Ult_left>14)&&(Ult_right<16)&&(Ult_right>14)) { //合った場合 |
Ryosei | 25:d367d1e7a153 | 960 | motor[TIRE_FL].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 961 | motor[TIRE_FR].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 962 | motor[TIRE_BL].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 963 | motor[TIRE_BR].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 964 | motor[TIRE_FL].pwm=255; |
Ryosei | 25:d367d1e7a153 | 965 | motor[TIRE_FR].pwm=255; |
Ryosei | 25:d367d1e7a153 | 966 | motor[TIRE_BL].pwm=255; |
Ryosei | 25:d367d1e7a153 | 967 | motor[TIRE_BR].pwm=255; |
Ryosei | 25:d367d1e7a153 | 968 | phase=3;//system phase increasing |
Ryosei | 25:d367d1e7a153 | 969 | } else if((Ult_left<16)&&(Ult_left>14)) { //Ult_leftのみあった場合 |
Ryosei | 25:d367d1e7a153 | 970 | if(Ult_right>16) { //Ult_rightが遠い場合 |
Ryosei | 25:d367d1e7a153 | 971 | motor[TIRE_FL].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 972 | motor[TIRE_FR].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 973 | motor[TIRE_BL].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 974 | motor[TIRE_BR].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 975 | motor[TIRE_FL].pwm=0; |
Ryosei | 25:d367d1e7a153 | 976 | motor[TIRE_FR].pwm=13; |
Ryosei | 25:d367d1e7a153 | 977 | motor[TIRE_BL].pwm=0; |
Ryosei | 25:d367d1e7a153 | 978 | motor[TIRE_BR].pwm=13; |
Ryosei | 25:d367d1e7a153 | 979 | } else if(Ult_right<14) { //Ult_rightが近い場合 |
Ryosei | 25:d367d1e7a153 | 980 | motor[TIRE_FL].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 981 | motor[TIRE_FR].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 982 | motor[TIRE_BL].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 983 | motor[TIRE_BR].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 984 | motor[TIRE_FL].pwm=0; |
Ryosei | 25:d367d1e7a153 | 985 | motor[TIRE_FR].pwm=13; |
Ryosei | 25:d367d1e7a153 | 986 | motor[TIRE_BL].pwm=0; |
Ryosei | 25:d367d1e7a153 | 987 | motor[TIRE_BR].pwm=13; |
Ryosei | 25:d367d1e7a153 | 988 | } |
Ryosei | 25:d367d1e7a153 | 989 | } else if((Ult_right<16)&&(Ult_right>14)) { //Ult_rightのみあった場合 |
Ryosei | 25:d367d1e7a153 | 990 | if(Ult_left>16) { //Ult_leftが遠い場合 |
Ryosei | 25:d367d1e7a153 | 991 | motor[TIRE_FL].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 992 | motor[TIRE_FR].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 993 | motor[TIRE_BL].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 994 | motor[TIRE_BR].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 995 | motor[TIRE_FL].pwm=13; |
Ryosei | 25:d367d1e7a153 | 996 | motor[TIRE_FR].pwm=0; |
Ryosei | 25:d367d1e7a153 | 997 | motor[TIRE_BL].pwm=13; |
Ryosei | 25:d367d1e7a153 | 998 | motor[TIRE_BR].pwm=0; |
Ryosei | 25:d367d1e7a153 | 999 | } else if(Ult_left<14) { //Ult_leftが近い場合 |
Ryosei | 25:d367d1e7a153 | 1000 | motor[TIRE_FL].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 1001 | motor[TIRE_FR].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 1002 | motor[TIRE_BL].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 1003 | motor[TIRE_BR].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 1004 | motor[TIRE_FL].pwm=13; |
Ryosei | 25:d367d1e7a153 | 1005 | motor[TIRE_FR].pwm=0; |
Ryosei | 25:d367d1e7a153 | 1006 | motor[TIRE_BL].pwm=13; |
Ryosei | 25:d367d1e7a153 | 1007 | motor[TIRE_BR].pwm=0; |
Ryosei | 25:d367d1e7a153 | 1008 | } |
Ryosei | 25:d367d1e7a153 | 1009 | } else { //どっちもあってない場合 |
Ryosei | 25:d367d1e7a153 | 1010 | if( (Ult_left+Ult_right)<=25||(Ult_left+Ult_right)>=35) {//離れすぎているor近すぎるとき |
Ryosei | 25:d367d1e7a153 | 1011 | if((Ult_left-Ult_right)>0||((Ult_left-Ult_right)<0) ) { //傾きが大きいとき |
Ryosei | 25:d367d1e7a153 | 1012 | if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき |
Ryosei | 25:d367d1e7a153 | 1013 | motor[TIRE_FL].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 1014 | motor[TIRE_FR].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 1015 | motor[TIRE_BL].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 1016 | motor[TIRE_BR].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 1017 | motor[TIRE_FL].pwm=10*Ult_left-Ult_right; |
Ryosei | 25:d367d1e7a153 | 1018 | motor[TIRE_FR].pwm=10*Ult_left-Ult_right; |
Ryosei | 25:d367d1e7a153 | 1019 | motor[TIRE_BL].pwm=10*Ult_left-Ult_right; |
Ryosei | 25:d367d1e7a153 | 1020 | motor[TIRE_BR].pwm=10*Ult_left-Ult_right; |
Ryosei | 25:d367d1e7a153 | 1021 | } else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき |
Ryosei | 25:d367d1e7a153 | 1022 | motor[TIRE_FL].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 1023 | motor[TIRE_FR].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 1024 | motor[TIRE_BL].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 1025 | motor[TIRE_BR].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 1026 | motor[TIRE_FL].pwm=10*Ult_left-Ult_right; |
Ryosei | 25:d367d1e7a153 | 1027 | motor[TIRE_FR].pwm=10*Ult_left-Ult_right; |
Ryosei | 25:d367d1e7a153 | 1028 | motor[TIRE_BL].pwm=10*Ult_left-Ult_right; |
Ryosei | 25:d367d1e7a153 | 1029 | motor[TIRE_BR].pwm=10*Ult_left-Ult_right; |
Ryosei | 25:d367d1e7a153 | 1030 | } |
Ryosei | 25:d367d1e7a153 | 1031 | } |
Ryosei | 25:d367d1e7a153 | 1032 | } else { //さほど離れてはいないが傾きが大きいとき |
Ryosei | 25:d367d1e7a153 | 1033 | if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき |
Ryosei | 25:d367d1e7a153 | 1034 | motor[TIRE_FL].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 1035 | motor[TIRE_FR].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 1036 | motor[TIRE_BL].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 1037 | motor[TIRE_BR].dir=FOR; |
Ryosei | 25:d367d1e7a153 | 1038 | motor[TIRE_FL].pwm=10*Ult_left-Ult_right; |
Ryosei | 25:d367d1e7a153 | 1039 | motor[TIRE_FR].pwm=10*Ult_left-Ult_right; |
Ryosei | 25:d367d1e7a153 | 1040 | motor[TIRE_BL].pwm=10*Ult_left-Ult_right; |
Ryosei | 25:d367d1e7a153 | 1041 | motor[TIRE_BR].pwm=10*Ult_left-Ult_right; |
Ryosei | 25:d367d1e7a153 | 1042 | } else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき |
Ryosei | 25:d367d1e7a153 | 1043 | motor[TIRE_FL].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 1044 | motor[TIRE_FR].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 1045 | motor[TIRE_BL].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 1046 | motor[TIRE_BR].dir=BACK; |
Ryosei | 25:d367d1e7a153 | 1047 | motor[TIRE_FL].pwm=10*Ult_left-Ult_right; |
Ryosei | 25:d367d1e7a153 | 1048 | motor[TIRE_FR].pwm=10*Ult_left-Ult_right; |
Ryosei | 25:d367d1e7a153 | 1049 | motor[TIRE_BL].pwm=10*Ult_left-Ult_right; |
Ryosei | 25:d367d1e7a153 | 1050 | motor[TIRE_BR].pwm=10*Ult_left-Ult_right; |
Ryosei | 25:d367d1e7a153 | 1051 | } |
Ryosei | 25:d367d1e7a153 | 1052 | } |
Ryosei | 25:d367d1e7a153 | 1053 | } |
Ryosei | 25:d367d1e7a153 | 1054 | |
Ryosei | 25:d367d1e7a153 | 1055 | } else {//データを受け取ってないとき |
Ryosei | 25:d367d1e7a153 | 1056 | motor[TIRE_FL].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 1057 | motor[TIRE_FR].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 1058 | motor[TIRE_BL].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 1059 | motor[TIRE_BR].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 1060 | motor[TIRE_FL].pwm=100; |
Ryosei | 25:d367d1e7a153 | 1061 | motor[TIRE_FR].pwm=100; |
Ryosei | 25:d367d1e7a153 | 1062 | motor[TIRE_BL].pwm=100; |
Ryosei | 25:d367d1e7a153 | 1063 | motor[TIRE_BR].pwm=100; |
Ryosei | 25:d367d1e7a153 | 1064 | } |
Ryosei | 25:d367d1e7a153 | 1065 | } else { |
Ryosei | 25:d367d1e7a153 | 1066 | //一応 |
Ryosei | 25:d367d1e7a153 | 1067 | motor[TIRE_FL].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 1068 | motor[TIRE_FR].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 1069 | motor[TIRE_BL].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 1070 | motor[TIRE_BR].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 1071 | motor[TIRE_FL].pwm=100; |
Ryosei | 25:d367d1e7a153 | 1072 | motor[TIRE_FR].pwm=100; |
Ryosei | 25:d367d1e7a153 | 1073 | motor[TIRE_BL].pwm=100; |
Ryosei | 25:d367d1e7a153 | 1074 | motor[TIRE_BR].pwm=100; |
Ryosei | 25:d367d1e7a153 | 1075 | } |
Ryosei | 25:d367d1e7a153 | 1076 | } else { |
Ryosei | 25:d367d1e7a153 | 1077 | motor[TIRE_FL].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 1078 | motor[TIRE_FR].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 1079 | motor[TIRE_BL].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 1080 | motor[TIRE_BR].dir=BRAKE; |
Ryosei | 25:d367d1e7a153 | 1081 | motor[TIRE_FL].pwm=100; |
Ryosei | 25:d367d1e7a153 | 1082 | motor[TIRE_FR].pwm=100; |
Ryosei | 25:d367d1e7a153 | 1083 | motor[TIRE_BL].pwm=100; |
Ryosei | 25:d367d1e7a153 | 1084 | motor[TIRE_BR].pwm=100; |
Ryosei | 25:d367d1e7a153 | 1085 | } |
kishibekairohan | 13:b6e02d6261d7 | 1086 | } |
kishibekairohan | 13:b6e02d6261d7 | 1087 | #endif |
kishibekairohan | 13:b6e02d6261d7 | 1088 | |
kishibekairohan | 13:b6e02d6261d7 | 1089 | #if USE_PROCESS_NUM>3 |
Ryosei | 25:d367d1e7a153 | 1090 | static void Process3() |
kishibekairohan | 13:b6e02d6261d7 | 1091 | { |
Ryosei | 25:d367d1e7a153 | 1092 | AllActuatorReset(); |
Ryosei | 25:d367d1e7a153 | 1093 | lineFase = 0; |
kishibekairohan | 13:b6e02d6261d7 | 1094 | } |
kishibekairohan | 13:b6e02d6261d7 | 1095 | #endif |
kishibekairohan | 13:b6e02d6261d7 | 1096 | |
kishibekairohan | 13:b6e02d6261d7 | 1097 | #if USE_PROCESS_NUM>4 |
Ryosei | 25:d367d1e7a153 | 1098 | static void Process4() |
M_souta | 22:7d93f79a3686 | 1099 | { |
Ryosei | 25:d367d1e7a153 | 1100 | |
Ryosei | 25:d367d1e7a153 | 1101 | static int x,y; |
Ryosei | 25:d367d1e7a153 | 1102 | static int count = 0; |
Ryosei | 25:d367d1e7a153 | 1103 | |
Ryosei | 25:d367d1e7a153 | 1104 | linePara_U = lineCast(LineHub::GetPara(0)); |
Ryosei | 25:d367d1e7a153 | 1105 | linePara_B = lineCast(LineHub::GetPara(2)); |
Ryosei | 25:d367d1e7a153 | 1106 | linePara_L = lineCast(LineHub::GetPara(3)); |
Ryosei | 25:d367d1e7a153 | 1107 | linePara_R = lineCast(LineHub::GetPara(4)); |
Ryosei | 25:d367d1e7a153 | 1108 | |
Ryosei | 25:d367d1e7a153 | 1109 | if(linePara_B == 'A' && count == 0) { |
Ryosei | 25:d367d1e7a153 | 1110 | lineFase = 1; |
Ryosei | 25:d367d1e7a153 | 1111 | } |
kishibekairohan | 15:dfcec98f5aa9 | 1112 | |
Ryosei | 25:d367d1e7a153 | 1113 | if(lineFase == 0) { |
Ryosei | 25:d367d1e7a153 | 1114 | pw = 0.5; |
Ryosei | 25:d367d1e7a153 | 1115 | switch(linePara_U) { |
Ryosei | 25:d367d1e7a153 | 1116 | case -2: |
Ryosei | 25:d367d1e7a153 | 1117 | x = 5; |
Ryosei | 25:d367d1e7a153 | 1118 | y = 3; |
Ryosei | 25:d367d1e7a153 | 1119 | break; |
Ryosei | 25:d367d1e7a153 | 1120 | case -3: |
Ryosei | 25:d367d1e7a153 | 1121 | x = 5; |
Ryosei | 25:d367d1e7a153 | 1122 | y = 3; |
Ryosei | 25:d367d1e7a153 | 1123 | break; |
Ryosei | 25:d367d1e7a153 | 1124 | case -1: |
Ryosei | 25:d367d1e7a153 | 1125 | x = 6; |
Ryosei | 25:d367d1e7a153 | 1126 | y = 3; |
Ryosei | 25:d367d1e7a153 | 1127 | break; |
Ryosei | 25:d367d1e7a153 | 1128 | case 0: |
Ryosei | 25:d367d1e7a153 | 1129 | x = 7; |
Ryosei | 25:d367d1e7a153 | 1130 | y = 3; |
Ryosei | 25:d367d1e7a153 | 1131 | break; |
Ryosei | 25:d367d1e7a153 | 1132 | case 1: |
Ryosei | 25:d367d1e7a153 | 1133 | x = 8; |
Ryosei | 25:d367d1e7a153 | 1134 | y = 3; |
Ryosei | 25:d367d1e7a153 | 1135 | break; |
Ryosei | 25:d367d1e7a153 | 1136 | case 3: |
Ryosei | 25:d367d1e7a153 | 1137 | x = 9; |
Ryosei | 25:d367d1e7a153 | 1138 | y = 3; |
Ryosei | 25:d367d1e7a153 | 1139 | break; |
Ryosei | 25:d367d1e7a153 | 1140 | case 2: |
Ryosei | 25:d367d1e7a153 | 1141 | x = 9; |
Ryosei | 25:d367d1e7a153 | 1142 | y = 3; |
Ryosei | 25:d367d1e7a153 | 1143 | break; |
Ryosei | 25:d367d1e7a153 | 1144 | case 'A': |
Ryosei | 25:d367d1e7a153 | 1145 | lineCheck = true; |
Ryosei | 25:d367d1e7a153 | 1146 | x = x; |
Ryosei | 25:d367d1e7a153 | 1147 | y = y; |
Ryosei | 25:d367d1e7a153 | 1148 | break; |
Ryosei | 25:d367d1e7a153 | 1149 | case 'N': |
Ryosei | 25:d367d1e7a153 | 1150 | x = 7; |
Ryosei | 25:d367d1e7a153 | 1151 | y = 7; |
Ryosei | 25:d367d1e7a153 | 1152 | break; |
Ryosei | 25:d367d1e7a153 | 1153 | x = 7; |
Ryosei | 25:d367d1e7a153 | 1154 | y = 7; |
Ryosei | 25:d367d1e7a153 | 1155 | default: |
Ryosei | 25:d367d1e7a153 | 1156 | x = 9; |
Ryosei | 25:d367d1e7a153 | 1157 | y = 9; |
Ryosei | 25:d367d1e7a153 | 1158 | } |
Ryosei | 25:d367d1e7a153 | 1159 | if(lineCheck == true && (!(linePara_U) == 'A')) { |
Ryosei | 25:d367d1e7a153 | 1160 | count++; |
Ryosei | 25:d367d1e7a153 | 1161 | } |
Ryosei | 25:d367d1e7a153 | 1162 | } else if(lineFase == 1) { |
Ryosei | 25:d367d1e7a153 | 1163 | pw = 0.3; |
Ryosei | 25:d367d1e7a153 | 1164 | x = 7; |
Ryosei | 25:d367d1e7a153 | 1165 | y = 9; |
Ryosei | 25:d367d1e7a153 | 1166 | if(linePara_R == 0) { |
Ryosei | 25:d367d1e7a153 | 1167 | lineFase = 2; |
Ryosei | 25:d367d1e7a153 | 1168 | x = 7; |
Ryosei | 25:d367d1e7a153 | 1169 | y = 7; |
Ryosei | 25:d367d1e7a153 | 1170 | } |
Ryosei | 25:d367d1e7a153 | 1171 | } else if(lineFase == 2) { |
Ryosei | 25:d367d1e7a153 | 1172 | x = 7; |
Ryosei | 25:d367d1e7a153 | 1173 | y = 7; |
Ryosei | 25:d367d1e7a153 | 1174 | } else { |
Ryosei | 25:d367d1e7a153 | 1175 | x = 7; |
Ryosei | 25:d367d1e7a153 | 1176 | y = 7; |
Ryosei | 25:d367d1e7a153 | 1177 | } |
Ryosei | 25:d367d1e7a153 | 1178 | |
Ryosei | 25:d367d1e7a153 | 1179 | int t = 0; |
Ryosei | 25:d367d1e7a153 | 1180 | if((linePara_U + linePara_B) > 3) t = 1; |
Ryosei | 25:d367d1e7a153 | 1181 | |
Ryosei | 25:d367d1e7a153 | 1182 | if(controller->Button.A) { |
Ryosei | 25:d367d1e7a153 | 1183 | motor[TIRE_BL].dir = SetStatus(-omni[y][14-x] ); |
Ryosei | 25:d367d1e7a153 | 1184 | motor[TIRE_FL].dir = SetStatus(omni[y][x]); |
Ryosei | 25:d367d1e7a153 | 1185 | motor[TIRE_BR].dir = SetStatus(-omni[14-x][14-y]); |
Ryosei | 25:d367d1e7a153 | 1186 | motor[TIRE_FR].dir = SetStatus(omni[x][14-y]); |
Ryosei | 25:d367d1e7a153 | 1187 | |
Ryosei | 25:d367d1e7a153 | 1188 | motor[TIRE_FR].pwm = SetPWM((omni[y][14-x])) * pw; |
Ryosei | 25:d367d1e7a153 | 1189 | motor[TIRE_FL].pwm = SetPWM((omni[y][x])) * pw; |
Ryosei | 25:d367d1e7a153 | 1190 | motor[TIRE_BR].pwm = SetPWM((omni[14-x][14-y])) * pw; |
Ryosei | 25:d367d1e7a153 | 1191 | motor[TIRE_BL].pwm = SetPWM((omni[x][14-y])) * pw; |
Ryosei | 25:d367d1e7a153 | 1192 | } else { |
Ryosei | 25:d367d1e7a153 | 1193 | motor[TIRE_BL].dir = SetStatus(0); |
Ryosei | 25:d367d1e7a153 | 1194 | motor[TIRE_FL].dir = SetStatus(0); |
Ryosei | 25:d367d1e7a153 | 1195 | motor[TIRE_BR].dir = SetStatus(0); |
Ryosei | 25:d367d1e7a153 | 1196 | motor[TIRE_FR].dir = SetStatus(0); |
Ryosei | 25:d367d1e7a153 | 1197 | |
Ryosei | 25:d367d1e7a153 | 1198 | motor[TIRE_FR].pwm = SetPWM(0); |
Ryosei | 25:d367d1e7a153 | 1199 | motor[TIRE_FL].pwm = SetPWM(0); |
Ryosei | 25:d367d1e7a153 | 1200 | motor[TIRE_BR].pwm = SetPWM(0); |
Ryosei | 25:d367d1e7a153 | 1201 | motor[TIRE_BL].pwm = SetPWM(0); |
Ryosei | 25:d367d1e7a153 | 1202 | } |
Ryosei | 25:d367d1e7a153 | 1203 | |
kishibekairohan | 13:b6e02d6261d7 | 1204 | } |
kishibekairohan | 13:b6e02d6261d7 | 1205 | #endif |
kishibekairohan | 13:b6e02d6261d7 | 1206 | |
kishibekairohan | 13:b6e02d6261d7 | 1207 | #if USE_PROCESS_NUM>5 |
Ryosei | 25:d367d1e7a153 | 1208 | static void Process5() |
Ryosei | 25:d367d1e7a153 | 1209 | { |
Ryosei | 25:d367d1e7a153 | 1210 | lineFase = 0; |
Ryosei | 25:d367d1e7a153 | 1211 | lineCheck = true; |
kishibekairohan | 13:b6e02d6261d7 | 1212 | } |
kishibekairohan | 13:b6e02d6261d7 | 1213 | #endif |
kishibekairohan | 13:b6e02d6261d7 | 1214 | |
kishibekairohan | 13:b6e02d6261d7 | 1215 | #if USE_PROCESS_NUM>6 |
Ryosei | 25:d367d1e7a153 | 1216 | static void Process6() |
kishibekairohan | 13:b6e02d6261d7 | 1217 | { |
Ryosei | 25:d367d1e7a153 | 1218 | |
Ryosei | 25:d367d1e7a153 | 1219 | for(int i = 0; i < 8; i++) { |
Ryosei | 25:d367d1e7a153 | 1220 | linePara[i] = lineCast(LineHub::GetPara(i)); |
Ryosei | 25:d367d1e7a153 | 1221 | } |
Ryosei | 25:d367d1e7a153 | 1222 | |
Ryosei | 25:d367d1e7a153 | 1223 | static int count = 100000; |
Ryosei | 25:d367d1e7a153 | 1224 | count++; |
Ryosei | 25:d367d1e7a153 | 1225 | |
Ryosei | 25:d367d1e7a153 | 1226 | if(count < 10000) { |
Ryosei | 25:d367d1e7a153 | 1227 | lineCheck = false; |
Ryosei | 25:d367d1e7a153 | 1228 | } else { |
Ryosei | 25:d367d1e7a153 | 1229 | lineCheck = true; |
Ryosei | 25:d367d1e7a153 | 1230 | } |
Ryosei | 25:d367d1e7a153 | 1231 | |
Ryosei | 25:d367d1e7a153 | 1232 | if(lineFase == 0) { // 前進 |
Ryosei | 25:d367d1e7a153 | 1233 | switch(linePara[0]) { |
Ryosei | 25:d367d1e7a153 | 1234 | motor[TIRE_FL].dir = FOR; |
Ryosei | 25:d367d1e7a153 | 1235 | motor[TIRE_BL].dir = BRAKE; |
Ryosei | 25:d367d1e7a153 | 1236 | motor[TIRE_BR].dir = BACK; |
Ryosei | 25:d367d1e7a153 | 1237 | motor[TIRE_FR].dir = BRAKE; |
Ryosei | 25:d367d1e7a153 | 1238 | motor[TIRE_FL].pwm = 30; |
Ryosei | 25:d367d1e7a153 | 1239 | motor[TIRE_FR].pwm = 0; |
Ryosei | 25:d367d1e7a153 | 1240 | motor[TIRE_BR].pwm = 30; |
Ryosei | 25:d367d1e7a153 | 1241 | motor[TIRE_BL].pwm = 0; |
Ryosei | 25:d367d1e7a153 | 1242 | break; |
Ryosei | 25:d367d1e7a153 | 1243 | case -3: |
Ryosei | 25:d367d1e7a153 | 1244 | motor[TIRE_FL].dir = FOR; |
Ryosei | 25:d367d1e7a153 | 1245 | motor[TIRE_BL].dir = BACK; |
Ryosei | 25:d367d1e7a153 | 1246 | motor[TIRE_BR].dir = BACK; |
Ryosei | 25:d367d1e7a153 | 1247 | motor[TIRE_FR].dir = FOR; |
Ryosei | 25:d367d1e7a153 | 1248 | motor[TIRE_FL].pwm = 30; |
Ryosei | 25:d367d1e7a153 | 1249 | motor[TIRE_FR].pwm = 10; |
Ryosei | 25:d367d1e7a153 | 1250 | motor[TIRE_BR].pwm = 30; |
Ryosei | 25:d367d1e7a153 | 1251 | motor[TIRE_BL].pwm = 10; |
Ryosei | 25:d367d1e7a153 | 1252 | break; |
Ryosei | 25:d367d1e7a153 | 1253 | case -1: |
Ryosei | 25:d367d1e7a153 | 1254 | motor[TIRE_FL].dir = FOR; |
Ryosei | 25:d367d1e7a153 | 1255 | motor[TIRE_BL].dir = BACK; |
Ryosei | 25:d367d1e7a153 | 1256 | motor[TIRE_BR].dir = BACK; |
Ryosei | 25:d367d1e7a153 | 1257 | motor[TIRE_FR].dir = FOR; |
Ryosei | 25:d367d1e7a153 | 1258 | motor[TIRE_FL].pwm = 30; |
Ryosei | 25:d367d1e7a153 | 1259 | motor[TIRE_FR].pwm = 20; |
Ryosei | 25:d367d1e7a153 | 1260 | motor[TIRE_BR].pwm = 30; |
Ryosei | 25:d367d1e7a153 | 1261 | motor[TIRE_BL].pwm = 20; |
Ryosei | 25:d367d1e7a153 | 1262 | break; |
Ryosei | 25:d367d1e7a153 | 1263 | case 0: |
Ryosei | 25:d367d1e7a153 | 1264 | motor[TIRE_FL].dir = FOR; |
Ryosei | 25:d367d1e7a153 | 1265 | motor[TIRE_BL].dir = BACK; |
Ryosei | 25:d367d1e7a153 | 1266 | motor[TIRE_BR].dir = BACK; |
Ryosei | 25:d367d1e7a153 | 1267 | motor[TIRE_FR].dir = FOR; |
Ryosei | 25:d367d1e7a153 | 1268 | motor[TIRE_FL].pwm = 30; |
Ryosei | 25:d367d1e7a153 | 1269 | motor[TIRE_FR].pwm = 30; |
Ryosei | 25:d367d1e7a153 | 1270 | motor[TIRE_BR].pwm = 30; |
Ryosei | 25:d367d1e7a153 | 1271 | motor[TIRE_BL].pwm = 30; |
Ryosei | 25:d367d1e7a153 | 1272 | break; |
Ryosei | 25:d367d1e7a153 | 1273 | case 1: |
Ryosei | 25:d367d1e7a153 | 1274 | motor[TIRE_FL].dir = FOR; |
Ryosei | 25:d367d1e7a153 | 1275 | motor[TIRE_BL].dir = BACK; |
Ryosei | 25:d367d1e7a153 | 1276 | motor[TIRE_BR].dir = BACK; |
Ryosei | 25:d367d1e7a153 | 1277 | motor[TIRE_FR].dir = FOR; |
Ryosei | 25:d367d1e7a153 | 1278 | motor[TIRE_FL].pwm = 20; |
Ryosei | 25:d367d1e7a153 | 1279 | motor[TIRE_FR].pwm = 30; |
Ryosei | 25:d367d1e7a153 | 1280 | motor[TIRE_BR].pwm = 20; |
Ryosei | 25:d367d1e7a153 | 1281 | motor[TIRE_BL].pwm = 30; |
Ryosei | 25:d367d1e7a153 | 1282 | break; |
Ryosei | 25:d367d1e7a153 | 1283 | case 3: |
Ryosei | 25:d367d1e7a153 | 1284 | motor[TIRE_FL].dir = FOR; |
Ryosei | 25:d367d1e7a153 | 1285 | motor[TIRE_BL].dir = FOR; |
Ryosei | 25:d367d1e7a153 | 1286 | motor[TIRE_BR].dir = BACK; |
Ryosei | 25:d367d1e7a153 | 1287 | motor[TIRE_FR].dir = BACK; |
Ryosei | 25:d367d1e7a153 | 1288 | motor[TIRE_FL].pwm = 10; |
Ryosei | 25:d367d1e7a153 | 1289 | motor[TIRE_FR].pwm = 30; |
Ryosei | 25:d367d1e7a153 | 1290 | motor[TIRE_BR].pwm = 10; |
Ryosei | 25:d367d1e7a153 | 1291 | motor[TIRE_BL].pwm = 30; |
Ryosei | 25:d367d1e7a153 | 1292 | break; |
Ryosei | 25:d367d1e7a153 | 1293 | case 2: |
Ryosei | 25:d367d1e7a153 | 1294 | motor[TIRE_FL].dir = BRAKE; |
Ryosei | 25:d367d1e7a153 | 1295 | motor[TIRE_BL].dir = FOR; |
Ryosei | 25:d367d1e7a153 | 1296 | motor[TIRE_BR].dir = BRAKE; |
Ryosei | 25:d367d1e7a153 | 1297 | motor[TIRE_FR].dir = BACK; |
Ryosei | 25:d367d1e7a153 | 1298 | motor[TIRE_FL].pwm = 0; |
Ryosei | 25:d367d1e7a153 | 1299 | motor[TIRE_FR].pwm = 30; |
Ryosei | 25:d367d1e7a153 | 1300 | motor[TIRE_BR].pwm = 0; |
Ryosei | 25:d367d1e7a153 | 1301 | motor[TIRE_BL].pwm = 30; |
Ryosei | 25:d367d1e7a153 | 1302 | break; |
Ryosei | 25:d367d1e7a153 | 1303 | case 'A': |
Ryosei | 25:d367d1e7a153 | 1304 | motor[TIRE_FL].dir = FOR; |
Ryosei | 25:d367d1e7a153 | 1305 | motor[TIRE_BL].dir = BACK; |
Ryosei | 25:d367d1e7a153 | 1306 | motor[TIRE_BR].dir = BACK; |
Ryosei | 25:d367d1e7a153 | 1307 | motor[TIRE_FR].dir = FOR; |
Ryosei | 25:d367d1e7a153 | 1308 | motor[TIRE_FL].pwm = 30; |
Ryosei | 25:d367d1e7a153 | 1309 | motor[TIRE_FR].pwm = 30; |
Ryosei | 25:d367d1e7a153 | 1310 | motor[TIRE_BR].pwm = 30; |
Ryosei | 25:d367d1e7a153 | 1311 | motor[TIRE_BL].pwm = 30; |
Ryosei | 25:d367d1e7a153 | 1312 | if(lineCheck == true) { |
Ryosei | 25:d367d1e7a153 | 1313 | lineCount++; |
Ryosei | 25:d367d1e7a153 | 1314 | count = 0; |
Ryosei | 25:d367d1e7a153 | 1315 | } |
Ryosei | 25:d367d1e7a153 | 1316 | default: |
Ryosei | 25:d367d1e7a153 | 1317 | motor[TIRE_FL].dir = BRAKE; |
Ryosei | 25:d367d1e7a153 | 1318 | motor[TIRE_BL].dir = BRAKE; |
Ryosei | 25:d367d1e7a153 | 1319 | motor[TIRE_BR].dir = BRAKE; |
Ryosei | 25:d367d1e7a153 | 1320 | motor[TIRE_FR].dir = BRAKE; |
Ryosei | 25:d367d1e7a153 | 1321 | motor[TIRE_FL].pwm = 30; |
Ryosei | 25:d367d1e7a153 | 1322 | motor[TIRE_FR].pwm = 30; |
Ryosei | 25:d367d1e7a153 | 1323 | motor[TIRE_BR].pwm = 30; |
Ryosei | 25:d367d1e7a153 | 1324 | motor[TIRE_BL].pwm = 30; |
Ryosei | 25:d367d1e7a153 | 1325 | } |
Ryosei | 25:d367d1e7a153 | 1326 | if(lineCount == 1) { |
Ryosei | 25:d367d1e7a153 | 1327 | lineFase = 1; |
Ryosei | 25:d367d1e7a153 | 1328 | } |
Ryosei | 25:d367d1e7a153 | 1329 | } else if(lineFase == 1) { // 前進 低速 |
Ryosei | 25:d367d1e7a153 | 1330 | motor[TIRE_FL].dir = FOR; |
Ryosei | 25:d367d1e7a153 | 1331 | motor[TIRE_BL].dir = FOR; |
Ryosei | 25:d367d1e7a153 | 1332 | motor[TIRE_BR].dir = BACK; |
Ryosei | 25:d367d1e7a153 | 1333 | motor[TIRE_FR].dir = BACK; |
Ryosei | 25:d367d1e7a153 | 1334 | motor[TIRE_FL].pwm = 15; |
Ryosei | 25:d367d1e7a153 | 1335 | motor[TIRE_FR].pwm = 15; |
Ryosei | 25:d367d1e7a153 | 1336 | motor[TIRE_BR].pwm = 15; |
Ryosei | 25:d367d1e7a153 | 1337 | motor[TIRE_BL].pwm = 15; |
Ryosei | 25:d367d1e7a153 | 1338 | if(linePara[4] == 0) { |
Ryosei | 25:d367d1e7a153 | 1339 | lineFase = 2; |
Ryosei | 25:d367d1e7a153 | 1340 | motor[TIRE_FL].dir = BRAKE; |
Ryosei | 25:d367d1e7a153 | 1341 | motor[TIRE_BL].dir = BRAKE; |
Ryosei | 25:d367d1e7a153 | 1342 | motor[TIRE_BR].dir = BRAKE; |
Ryosei | 25:d367d1e7a153 | 1343 | motor[TIRE_FR].dir = BRAKE; |
Ryosei | 25:d367d1e7a153 | 1344 | motor[TIRE_FL].pwm = 30; |
Ryosei | 25:d367d1e7a153 | 1345 | motor[TIRE_FR].pwm = 30; |
Ryosei | 25:d367d1e7a153 | 1346 | motor[TIRE_BR].pwm = 30; |
Ryosei | 25:d367d1e7a153 | 1347 | motor[TIRE_BL].pwm = 30; |
Ryosei | 25:d367d1e7a153 | 1348 | } |
Ryosei | 25:d367d1e7a153 | 1349 | } else if(lineFase == 2) { // 位置調整 |
Ryosei | 25:d367d1e7a153 | 1350 | lineFase = 3; |
Ryosei | 25:d367d1e7a153 | 1351 | } else if(lineFase == 3) { // 右 直進 |
Ryosei | 25:d367d1e7a153 | 1352 | motor[TIRE_FL].dir = BRAKE; |
Ryosei | 25:d367d1e7a153 | 1353 | motor[TIRE_BL].dir = BRAKE; |
Ryosei | 25:d367d1e7a153 | 1354 | motor[TIRE_BR].dir = BRAKE; |
Ryosei | 25:d367d1e7a153 | 1355 | motor[TIRE_FR].dir = BRAKE; |
Ryosei | 25:d367d1e7a153 | 1356 | motor[TIRE_FL].pwm = 30; |
Ryosei | 25:d367d1e7a153 | 1357 | motor[TIRE_FR].pwm = 30; |
Ryosei | 25:d367d1e7a153 | 1358 | motor[TIRE_BR].pwm = 30; |
Ryosei | 25:d367d1e7a153 | 1359 | motor[TIRE_BL].pwm = 30; |
Ryosei | 25:d367d1e7a153 | 1360 | } |
Ryosei | 25:d367d1e7a153 | 1361 | |
Ryosei | 25:d367d1e7a153 | 1362 | } |
Ryosei | 25:d367d1e7a153 | 1363 | #endif |
Ryosei | 25:d367d1e7a153 | 1364 | Timer samp; |
Ryosei | 25:d367d1e7a153 | 1365 | #if USE_PROCESS_NUM>7 |
Ryosei | 25:d367d1e7a153 | 1366 | static void Process7() |
Ryosei | 25:d367d1e7a153 | 1367 | { |
Ryosei | 25:d367d1e7a153 | 1368 | ; |
Ryosei | 25:d367d1e7a153 | 1369 | static int flag=0; |
Ryosei | 25:d367d1e7a153 | 1370 | static int FL_PWM=0; |
Ryosei | 25:d367d1e7a153 | 1371 | static int FR_PWM=0; |
Ryosei | 25:d367d1e7a153 | 1372 | static int BL_PWM=0; |
Ryosei | 25:d367d1e7a153 | 1373 | static int BR_PWM=0; |
Ryosei | 25:d367d1e7a153 | 1374 | int targetpulse=1000; |
Ryosei | 25:d367d1e7a153 | 1375 | |
Ryosei | 25:d367d1e7a153 | 1376 | if(flag==0) { |
Ryosei | 25:d367d1e7a153 | 1377 | |
Ryosei | 25:d367d1e7a153 | 1378 | samp.start(); |
Ryosei | 25:d367d1e7a153 | 1379 | |
Ryosei | 25:d367d1e7a153 | 1380 | flag=1; |
Ryosei | 25:d367d1e7a153 | 1381 | } else { |
Ryosei | 25:d367d1e7a153 | 1382 | int pulseFL = -encoder[FL].getPulses()/samp.read(); |
Ryosei | 25:d367d1e7a153 | 1383 | int pulseBL = -encoder[BL].getPulses()/samp.read(); |
Ryosei | 25:d367d1e7a153 | 1384 | int pulseBR = -encoder[BR].getPulses()/samp.read(); |
Ryosei | 25:d367d1e7a153 | 1385 | int pulseFR = -encoder[FR].getPulses()/samp.read(); |
Ryosei | 25:d367d1e7a153 | 1386 | pc.printf("%f ,pulseFL:%d\r\n",samp.read(),pulseFL); |
Ryosei | 25:d367d1e7a153 | 1387 | |
Ryosei | 25:d367d1e7a153 | 1388 | FL_PWM=FL_PWM+=0.01*(targetpulse-pulseFL); |
Ryosei | 25:d367d1e7a153 | 1389 | FR_PWM=FR_PWM+=0.01*(targetpulse-pulseFR); |
Ryosei | 25:d367d1e7a153 | 1390 | BL_PWM=BL_PWM+=0.01*(targetpulse-pulseBL); |
Ryosei | 25:d367d1e7a153 | 1391 | BR_PWM=BR_PWM+=0.01*(targetpulse-pulseBR); |
Ryosei | 25:d367d1e7a153 | 1392 | motor[TIRE_FL].dir = FOR; |
Ryosei | 25:d367d1e7a153 | 1393 | motor[TIRE_BL].dir = FOR; |
Ryosei | 25:d367d1e7a153 | 1394 | motor[TIRE_BR].dir = FOR; |
Ryosei | 25:d367d1e7a153 | 1395 | motor[TIRE_FR].dir = FOR; |
Ryosei | 25:d367d1e7a153 | 1396 | motor[TIRE_FL].pwm = FL_PWM; |
Ryosei | 25:d367d1e7a153 | 1397 | motor[TIRE_FR].pwm = FR_PWM; |
Ryosei | 25:d367d1e7a153 | 1398 | motor[TIRE_BR].pwm = BR_PWM; |
Ryosei | 25:d367d1e7a153 | 1399 | motor[TIRE_BL].pwm = BL_PWM; |
Ryosei | 25:d367d1e7a153 | 1400 | } |
kishibekairohan | 13:b6e02d6261d7 | 1401 | } |
kishibekairohan | 13:b6e02d6261d7 | 1402 | #endif |
kishibekairohan | 13:b6e02d6261d7 | 1403 | |
Ryosei | 25:d367d1e7a153 | 1404 | #if USE_PROCESS_NUM>8 |
kishibekairohan | 13:b6e02d6261d7 | 1405 | static void Process8() |
kishibekairohan | 12:c09b3e08a316 | 1406 | { |
Ryosei | 25:d367d1e7a153 | 1407 | if(controller->Button.A) { |
Ryosei | 25:d367d1e7a153 | 1408 | rotaconSampling.start(); |
Ryosei | 25:d367d1e7a153 | 1409 | PIDflag = true; |
Ryosei | 25:d367d1e7a153 | 1410 | |
Ryosei | 25:d367d1e7a153 | 1411 | //linePara_U = LineHub::GetPara(0); |
Ryosei | 25:d367d1e7a153 | 1412 | //linePara_B = LineHub::GetPara(3); |
Ryosei | 25:d367d1e7a153 | 1413 | |
Ryosei | 25:d367d1e7a153 | 1414 | |
Ryosei | 25:d367d1e7a153 | 1415 | pulsePV[FL] = encoder[FL].getPulses(); |
Ryosei | 25:d367d1e7a153 | 1416 | pulsePV[BL] = encoder[BL].getPulses(); |
Ryosei | 25:d367d1e7a153 | 1417 | pulsePV[BR] = encoder[BR].getPulses(); |
Ryosei | 25:d367d1e7a153 | 1418 | pulsePV[FR] = encoder[FR].getPulses(); |
Ryosei | 25:d367d1e7a153 | 1419 | |
Ryosei | 25:d367d1e7a153 | 1420 | |
Ryosei | 25:d367d1e7a153 | 1421 | for (int i = 0; i < 4; i++) { |
Ryosei | 25:d367d1e7a153 | 1422 | timeCV[i] = timePV[i]; |
Ryosei | 25:d367d1e7a153 | 1423 | timePV[i] = rotaconSampling.read(); |
Ryosei | 25:d367d1e7a153 | 1424 | tireProcessRPM[i] = (pulsePV[i] - pulseCV[i])/ (float)(256 * 2) / (timePV[i] - timeCV[i]) * 60; |
Ryosei | 25:d367d1e7a153 | 1425 | pulseCV[i] = pulsePV[i]; |
Ryosei | 25:d367d1e7a153 | 1426 | } |
Ryosei | 25:d367d1e7a153 | 1427 | |
Ryosei | 25:d367d1e7a153 | 1428 | move.Vx = 0.5; |
Ryosei | 25:d367d1e7a153 | 1429 | move.Vy = 0.5; |
Ryosei | 25:d367d1e7a153 | 1430 | move.Va = 0; |
Ryosei | 25:d367d1e7a153 | 1431 | |
Ryosei | 25:d367d1e7a153 | 1432 | correction_LT.Vx = 0; //0.1 * linePara_U; |
Ryosei | 25:d367d1e7a153 | 1433 | correction_LT.Vy = 0; |
Ryosei | 25:d367d1e7a153 | 1434 | correction_LT.Va = 0; |
Ryosei | 25:d367d1e7a153 | 1435 | |
Ryosei | 25:d367d1e7a153 | 1436 | synthetic.Vx = move.Vx + correction_LT.Vx; |
Ryosei | 25:d367d1e7a153 | 1437 | synthetic.Vy = move.Vy + correction_LT.Vy; |
Ryosei | 25:d367d1e7a153 | 1438 | synthetic.Va = move.Va + correction_LT.Va; |
Ryosei | 25:d367d1e7a153 | 1439 | |
Ryosei | 25:d367d1e7a153 | 1440 | sita = 0; |
Ryosei | 25:d367d1e7a153 | 1441 | |
Ryosei | 25:d367d1e7a153 | 1442 | //タイヤの目標速度算出 |
Ryosei | 25:d367d1e7a153 | 1443 | float sinR = 0.7071f * (float)sin(sita); |
Ryosei | 25:d367d1e7a153 | 1444 | float cosR = 0.7071f * (float)cos(sita); |
Ryosei | 25:d367d1e7a153 | 1445 | float nv = (60 * 1000) / ( 2.00 * PI * tireR); |
Ryosei | 25:d367d1e7a153 | 1446 | tireTargetRPM[FL] = ((+ synthetic.Vx * (sinR - cosR)) - (synthetic.Vy * (sinR + cosR)) + (ucR * synthetic.Va)) * nv; |
Ryosei | 25:d367d1e7a153 | 1447 | tireTargetRPM[BL] = ((+ synthetic.Vx * (sinR + cosR)) + (synthetic.Vy * (sinR - cosR)) + (ucR * synthetic.Va)) * nv; |
Ryosei | 25:d367d1e7a153 | 1448 | tireTargetRPM[BR] = ((- synthetic.Vx * (sinR - cosR)) + (synthetic.Vy * (sinR + cosR)) + (ucR * synthetic.Va)) * nv; |
Ryosei | 25:d367d1e7a153 | 1449 | tireTargetRPM[FR] = ((- synthetic.Vx * (sinR + cosR)) - (synthetic.Vy * (sinR - cosR)) + (ucR * synthetic.Va)) * nv; |
Ryosei | 25:d367d1e7a153 | 1450 | |
Ryosei | 25:d367d1e7a153 | 1451 | //pc.printf("process : %f target : %f\n\r",tireProcessRPM[0],tireTargetRPM[0]); |
Ryosei | 25:d367d1e7a153 | 1452 | |
Ryosei | 25:d367d1e7a153 | 1453 | //PIDによるPWM算出 |
Ryosei | 25:d367d1e7a153 | 1454 | |
Ryosei | 25:d367d1e7a153 | 1455 | //モータの駆動 |
Ryosei | 25:d367d1e7a153 | 1456 | for (int i = 0; i < 4; i++) { |
Ryosei | 25:d367d1e7a153 | 1457 | if (tirePWM[i] > 255) { |
Ryosei | 25:d367d1e7a153 | 1458 | tirePWM[i] = 255; |
Ryosei | 25:d367d1e7a153 | 1459 | } else if (tirePWM[i] < -255) { |
Ryosei | 25:d367d1e7a153 | 1460 | tirePWM[i] = -255; |
Ryosei | 25:d367d1e7a153 | 1461 | } |
Ryosei | 25:d367d1e7a153 | 1462 | } |
Ryosei | 25:d367d1e7a153 | 1463 | |
Ryosei | 25:d367d1e7a153 | 1464 | for(int i = 0; i < 4; i++) { |
Ryosei | 25:d367d1e7a153 | 1465 | motor[i].dir = SetStatus(tirePWM[i]); |
Ryosei | 25:d367d1e7a153 | 1466 | motor[i].pwm = SetPWM(tirePWM[i]); |
Ryosei | 25:d367d1e7a153 | 1467 | } |
Ryosei | 25:d367d1e7a153 | 1468 | } else { |
Ryosei | 25:d367d1e7a153 | 1469 | PIDflag = false; |
Ryosei | 25:d367d1e7a153 | 1470 | rotaconSampling.stop(); |
Ryosei | 25:d367d1e7a153 | 1471 | rotaconSampling.reset(); |
Ryosei | 25:d367d1e7a153 | 1472 | for(int i = 0; i < 4; i++) { |
Ryosei | 25:d367d1e7a153 | 1473 | encoder[i].reset(); |
Ryosei | 25:d367d1e7a153 | 1474 | pulsePV[i] = 0; |
Ryosei | 25:d367d1e7a153 | 1475 | pulseCV[i] = 0; |
Ryosei | 25:d367d1e7a153 | 1476 | timePV[i] = 0; |
Ryosei | 25:d367d1e7a153 | 1477 | timeCV[i] = 0; |
Ryosei | 25:d367d1e7a153 | 1478 | tirePWM[i] = 0; |
Ryosei | 25:d367d1e7a153 | 1479 | motor[i].dir = SetStatus(tirePWM[i]); |
Ryosei | 25:d367d1e7a153 | 1480 | motor[i].pwm = SetPWM(tirePWM[i]); |
Ryosei | 25:d367d1e7a153 | 1481 | } |
Ryosei | 25:d367d1e7a153 | 1482 | } |
kishibekairohan | 12:c09b3e08a316 | 1483 | } |
kishibekairohan | 12:c09b3e08a316 | 1484 | #endif |
kishibekairohan | 12:c09b3e08a316 | 1485 | |
kishibekairohan | 13:b6e02d6261d7 | 1486 | #if USE_PROCESS_NUM>9 |
kishibekairohan | 13:b6e02d6261d7 | 1487 | static void Process9() |
kishibekairohan | 12:c09b3e08a316 | 1488 | { |
Ryosei | 25:d367d1e7a153 | 1489 | |
t_yamamoto | 0:669ef71cba68 | 1490 | } |
t_yamamoto | 0:669ef71cba68 | 1491 | #endif |
t_yamamoto | 0:669ef71cba68 | 1492 | #endif |
t_yamamoto | 0:669ef71cba68 | 1493 | #pragma endregion PROCESS |
t_yamamoto | 0:669ef71cba68 | 1494 | |
t_yamamoto | 0:669ef71cba68 | 1495 | static void AllActuatorReset() |
t_yamamoto | 0:669ef71cba68 | 1496 | { |
t_yamamoto | 0:669ef71cba68 | 1497 | |
Ryosei | 25:d367d1e7a153 | 1498 | #ifdef USE_SOLENOID |
Ryosei | 25:d367d1e7a153 | 1499 | solenoid.all = ALL_SOLENOID_OFF; |
Ryosei | 25:d367d1e7a153 | 1500 | #endif |
t_yamamoto | 0:669ef71cba68 | 1501 | |
Ryosei | 25:d367d1e7a153 | 1502 | #ifdef USE_MOTOR |
Ryosei | 25:d367d1e7a153 | 1503 | for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) { |
Ryosei | 25:d367d1e7a153 | 1504 | motor[i].dir = FREE; |
Ryosei | 25:d367d1e7a153 | 1505 | motor[i].pwm = 0; |
Ryosei | 25:d367d1e7a153 | 1506 | } |
Ryosei | 25:d367d1e7a153 | 1507 | #endif |
t_yamamoto | 0:669ef71cba68 | 1508 | } |
t_yamamoto | 0:669ef71cba68 | 1509 | |
Ryosei | 25:d367d1e7a153 | 1510 | void BuzzerTimer_func() |
Ryosei | 25:d367d1e7a153 | 1511 | { |
kishibekairohan | 16:3f2c2d89372b | 1512 | buzzer = !buzzer; |
kishibekairohan | 17:50dc4b449e69 | 1513 | //LED_DEBUG0 = !LED_DEBUG0; |
kishibekairohan | 11:028a150943b5 | 1514 | } |
kishibekairohan | 11:028a150943b5 | 1515 | |
Ryosei | 25:d367d1e7a153 | 1516 | void TapeLedEms_func() |
Ryosei | 25:d367d1e7a153 | 1517 | { |
kishibekairohan | 16:3f2c2d89372b | 1518 | sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red; |
kishibekairohan | 9:f93fc79a49ea | 1519 | } |
kishibekairohan | 16:3f2c2d89372b | 1520 | |
kishibekairohan | 16:3f2c2d89372b | 1521 | #pragma region USER-DEFINED-FUNCTIONS |
Ryosei | 25:d367d1e7a153 | 1522 | void tirePID() |
Ryosei | 25:d367d1e7a153 | 1523 | { |
Ryosei | 25:d367d1e7a153 | 1524 | if(PIDflag == true) { |
Ryosei | 25:d367d1e7a153 | 1525 | //加算するPID値の算出 |
Ryosei | 25:d367d1e7a153 | 1526 | rotaconPID[0].SetPV(tireProcessRPM[FL],tireTargetRPM[FL]); |
Ryosei | 25:d367d1e7a153 | 1527 | rotaconPID[1].SetPV(tireProcessRPM[BL],tireTargetRPM[BL]); |
Ryosei | 25:d367d1e7a153 | 1528 | rotaconPID[2].SetPV(tireProcessRPM[FR],tireTargetRPM[FR]); |
Ryosei | 25:d367d1e7a153 | 1529 | rotaconPID[3].SetPV(tireProcessRPM[BR],tireTargetRPM[BR]); |
Ryosei | 25:d367d1e7a153 | 1530 | //PID値の加算 |
Ryosei | 25:d367d1e7a153 | 1531 | tirePWM[FL] += rotaconPID[0].GetMV(); |
Ryosei | 25:d367d1e7a153 | 1532 | tirePWM[BL] += rotaconPID[1].GetMV(); |
Ryosei | 25:d367d1e7a153 | 1533 | tirePWM[FR] += rotaconPID[2].GetMV(); |
Ryosei | 25:d367d1e7a153 | 1534 | tirePWM[BR] += rotaconPID[3].GetMV(); |
Ryosei | 25:d367d1e7a153 | 1535 | } |
M_souta | 22:7d93f79a3686 | 1536 | } |
kishibekairohan | 16:3f2c2d89372b | 1537 | |
Ryosei | 25:d367d1e7a153 | 1538 | int lineCast(char k) |
Ryosei | 25:d367d1e7a153 | 1539 | { |
Ryosei | 25:d367d1e7a153 | 1540 | int l; |
Ryosei | 25:d367d1e7a153 | 1541 | switch(k) { |
Ryosei | 25:d367d1e7a153 | 1542 | case 255: |
Ryosei | 25:d367d1e7a153 | 1543 | l = -1; |
Ryosei | 25:d367d1e7a153 | 1544 | break; |
Ryosei | 25:d367d1e7a153 | 1545 | case 254: |
Ryosei | 25:d367d1e7a153 | 1546 | l = -2; |
Ryosei | 25:d367d1e7a153 | 1547 | break; |
Ryosei | 25:d367d1e7a153 | 1548 | case 253: |
Ryosei | 25:d367d1e7a153 | 1549 | l = -3; |
Ryosei | 25:d367d1e7a153 | 1550 | break; |
Ryosei | 25:d367d1e7a153 | 1551 | default: |
Ryosei | 25:d367d1e7a153 | 1552 | l = k; |
Ryosei | 25:d367d1e7a153 | 1553 | } |
Ryosei | 25:d367d1e7a153 | 1554 | return l; |
M_souta | 22:7d93f79a3686 | 1555 | } |
kishibekairohan | 16:3f2c2d89372b | 1556 | |
t_yamamoto | 0:669ef71cba68 | 1557 | #pragma endregion |