Miya Miyagawa / Mbed 2 deprecated AutoMaster_si-tu

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?

UserRevisionLine numberNew 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