ケンタ ミヤザキ
Dependencies: PID Serialservo TCS3200 XQ_servo encoder encoder2 mbed moter
Fork of
nhk_2018_undercarry_test09
by 
ケンタ ミヤザキ
Diff: main.cpp
- Revision:
- 6:ff7fd6556a81
- Parent:
- 5:c5a2a5cf600d
- Child:
- 7:f1a924244b76
--- a/main.cpp Sun Apr 01 04:11:13 2018 +0000
+++ b/main.cpp Sun Apr 01 11:58:23 2018 +0000
@@ -25,7 +25,7 @@
Serialservo servo1(PC_10, PC_11);//アームの肘
Serialservo servo2(PC_10, PC_11);//回転軸
XQ_servo servo_hand(PB_5);
-DigitalOut dpin(PC_9);
+DigitalOut throwing_relay(PC_9);
DigitalIn distance(PC_2);
DigitalIn redIn(PC_4);
DigitalIn blueIn(PB_2);
@@ -36,13 +36,6 @@
DigitalOut motor_dir2(PA_9);
PwmOut motor_pwm(PB_3);
-void linecheck(char *buff ,int data[2]);
-void LineCheck(int dmode);
-void syokikanomai(void);
-void syokikanomai2(void);
-void pre_touteki(void);
-void touteki(float armpower, int pos, bool thro_mode);
-
PwmOut pwm_pins[] = {
PwmOut( PC_8 ),
PwmOut( PC_6 ),
@@ -64,6 +57,13 @@
DigitalOut( PB_15)
};
+
+void linecheck(char *buff ,int data[2]);
+void LineCheck(int dmode);
+void syokikanomai(void);
+void syokikanomai2(void);
+void touteki(float armpower, int pos, bool thro_mode);
+
int crosscount = 0;
bool mode = true;//trueでラジコン、falseでオート
bool stopflag = true;//trueで機体停止
@@ -89,8 +89,6 @@
int directmode = STRAIGHT;
int state = START;
int times = 0;
-bool throwing = false;
-int receive = NOT_RECEIVE;
int redcount = 0;
int bluecount = 0;
int yellowcount = 0;
@@ -101,19 +99,16 @@
int main() {
timer.reset();
timer.stop();
- servo1.init(0);//アーム
- servo2.init(1);//回転軸
- dpin=0;
+ throwing_relay=0;
lamp=1;
air=1.0;
buzzer = 0.0;
- //motor_relay = 0.0;
micro_switch.mode(PullUp);
-
motor_pwm=0;
- motor_dir1=1; //dir1,dir2はこの組み合わせじゃないと死ぬ
+ motor_dir1=1;
motor_dir2=1;
-
+ servo1.init(0);//アーム
+ servo2.init(1);//回転軸
S_pid.init();
R_pid.init();
L_pid.init();
@@ -122,14 +117,15 @@
pwm_pins[2].period(0.00005);
pwm_pins[3].period(0.00005);
motor_pwm.period(0.00005);
+//------通信スピード----------------------------------------------
pc.baud(115200);
kbtpc.baud(9600);
master.frequency(100000);
- lamp = 1.0;
-//-------初期化----------------------------------------
+ lamp = 1.0;
+//-------初期化-------------------------------------------------
syokikanomai();
buzzer = 0;
-//-----------メインループ----------------------------------------め
+//-----------メインループ----------------------------------------
while (true) {
master.read(addr1,buff1,2);
linecheck(buff1,linedata_1);
@@ -157,12 +153,8 @@
directmode = RIGHT;
}else if(kbtread == LEFT){
directmode = LEFT;
- }else if(kbtread == MANUAL){
- state = MANUAL;
}else if(kbtread == START){
state = START;
- }else if(kbtread == START_TEST){
- state = START_TEST;
}
//--------------手動--------------------------------------
if(mode == true){
@@ -186,82 +178,17 @@
}else{
LineCheck(directmode);
switch(state){
-//------------------任意に進行方向を決めれる----------------------------------------
- case MANUAL:
- if(crosscount == 1){
- directmode = STOP;
- Stop(pwm_pins, dir_pins_1, dir_pins_2);
- crosscount = 0;
- }
- break;
-//------------------テスト用--------------------------------------------------------
- case START_TEST:
- if(crosscount == 0){
- directmode = RIGHT;
- }else if(crosscount == 1){
- directmode = STOP;
- state = STAND_BY_TEST;
- crosscount = 0;
- prelinedata = 0;
- }
- break;
-//------------------テスト用待機位置-----------------------------------------------
- case STAND_BY_TEST:
- directmode = STOP;
- if(distance == 1.0){
- led2 = 1.0;
- if(redIn == 1.0){
- buzzer = 1.0;
- redcount++;
- }else if(blueIn == 1.0){
- buzzer = 1.0;
- bluecount++;
- }else{
- buzzer = 0.0;
- }
- if(redcount >= 50){
- servo_hand.move(hand_tsukami_degree);
- //pre_touteki();
- }else if(bluecount >= 50){
- servo_hand.move(hand_tsukami_degree);
- //pre_touteki();
- }
- }else if(distance == 0.0 && redcount >= 50){
- servo2.move(koshi_nage_degree);
- state = THROWING_TEST;
- directmode = STOP;
- led2 = 0.0;
- redcount = 0;
- crosscount = 0;
- }else if(distance == 0.0 && bluecount >= 50){
- servo2.move(koshi_nage_degree);
- state = THROWING_TEST;
- directmode = STOP;
- bluecount = 0;
- led2 = 0.0;
- crosscount = 0;
- }
- break;
-//------------------------------------------------------------------------------
- case THROWING_TEST:
- if(crosscount == 0){
- directmode = LEFT;
- }else if(crosscount == 1){
- directmode = STOP;
- touteki(0.8, 190, 0);
- //syokikanomai();
- buzzer = 0.0;
- }
- break;
//------------------初期位置から待機位置1------------------------------------------
case START:
if(crosscount == 0){
directmode = STRAIGHT;
- }else if(crosscount == 1 && directmode == STRAIGHT){
- directmode = LEFT;
- prelinedata = 0;
- }else if(crosscount == 1 && directmode == LEFT){
- directmode = LEFT;
+ }else if(crosscount == 1){
+ if(directmode == STRAIGHT){
+ directmode = LEFT;
+ prelinedata = 0;
+ }else if(directmode == LEFT){
+ directmode = LEFT;
+ }
}else if(crosscount == 2){
directmode = STOP;
state = STAND_BY_1;
@@ -286,12 +213,15 @@
if(redcount >= 100 ){
servo_hand.move(hand_tsukami_degree);
servo1.move(hiji_tsukami_degree);
+ servo1.move(hiji_tsukami_degree);
}else if(bluecount >= 100){
servo_hand.move(hand_tsukami_degree);
servo1.move(hiji_tsukami_degree);
+ servo1.move(hiji_tsukami_degree);
}
}else if(distance == 0.0 && redcount >= 100){
servo2.move(koshi_nage_degree);
+ servo2.move(koshi_nage_degree);
state = RED_RECEIVE;
directmode = STOP;
redcount = 0;
@@ -319,7 +249,6 @@
syokikanomai2();
if(set_flag == true){
state = STAND_BY_1;
- //throwing = false;
crosscount = 0;
}
}
@@ -356,11 +285,13 @@
if(bluecount >= 100){
servo_hand.move(hand_tsukami_degree);
servo1.move(hiji_tsukami_degree);
+ servo1.move(hiji_tsukami_degree);
}else if(yellowcount >= 500){
air = 0.0;
}
}else if(distance == 0.0 && bluecount >= 100){
servo2.move(koshi_nage_degree);
+ servo2.move(koshi_nage_degree);
state = BLUE_RECEIVE_2;
directmode = STOP;
bluecount = 0;
@@ -467,22 +398,18 @@
}
//---------------初期化---------------------------
-
-
void syokikanomai(void){
- //timer.start();
- //int times = timer.read();
syokika_flag = false;
while (set_flag==false) {
encoder_count= Enc_Arm.read_rotate();
servo_hand.move(hand_uke_degree);
servo1.move(hiji_nage_degree);//退避
servo2.move(koshi_uke_degree);
- //motor_relay = 0;
+ servo1.move(hiji_nage_degree);//退避
+ servo2.move(koshi_uke_degree);
motor_dir1=0;
motor_dir2=1;
motor_pwm=0.15;
-
if(Enc_Arm.read_z() == 1) {
syokika_flag=true;
buzzer = 1.0;
@@ -497,34 +424,25 @@
}
}
}
- //if(times >= 1.0){
- //wait(0.5);
- //servo_hand.move(hand_uke_degree);
servo1.move(hiji_uke_degree);
servo1.move(hiji_uke_degree);
- //servo2.move(koshi_uke_degree);
- //}
}
void syokikanomai2(void){
- //timer.start();
- //int times = timer.read();
- //syokika_flag = false;
if(set_flag==false) {
encoder_count= Enc_Arm.read_rotate();
servo_hand.move(hand_uke_degree);
servo1.move(hiji_nage_degree);//退避
servo2.move(koshi_uke_degree);
- //motor_relay = 0;
+ servo1.move(hiji_nage_degree);//退避
+ servo2.move(koshi_uke_degree);
motor_dir1=0;
motor_dir2=1;
motor_pwm=0.15;
-
if(Enc_Arm.read_z() == 1) {
syokika_flag = true;
buzzer = 1.0;
}
-
if (syokika_flag==true) {
if(encoder_count == 170) {
motor_pwm=0;
@@ -532,54 +450,23 @@
motor_dir2=1;
servo1.move(hiji_uke_degree);
servo1.move(hiji_uke_degree);
- //syokika_flag = false;
set_flag = true; //投てき後，falseにする.
}
}
}
- //if(times >= 1.0){
- //wait(0.5);
- //servo_hand.move(hand_uke_degree);
- //servo2.move(koshi_uke_degree);
- //}
}
-void pre_touteki(void){
- /*
- while(Enc_Arm.read_rotate()<pos) {
- motor_dir1 = 0;
- motor_dir2 = 1;
- motor_pwm = 0.15;
- }
- motor_dir1=1;
- motor_dir2=1;
- motor_pwm=0;
- */
- //wait(1.0);
- servo_hand.move(hand_tsukami_degree);
-
- //wait(1.0);
- servo2.move(koshi_nage_degree);
- //servo1.move(hiji_tsukami_degree);
-}
-
-void touteki(float armpower ,int pos, bool thro_mode){
+void touteki(float armpower ,int pos, bool throwing_mode){
if(set_flag == true){
timer.start();
}
int times = timer.read();
- //wait(1.0);
if(times >= 1.0 && times < 2.0){
servo_hand.move(hand_uke_degree);
- }
- //wait(1.0);
- //lamp = 0.0;
-
+ }
if(times >= 2.0 && times < 3.0){
servo1.move(hiji_nage_degree);
}
-
- //wait(1.0);
if(times >= 3.0 && times < 5.0){
lamp = 0.0;
while(Enc_Arm.read_rotate() < pos) {
@@ -591,22 +478,20 @@
motor_dir1=1;
motor_dir2=1;
motor_pwm=0;
- //wait(2.0);
//----投てき-------------------------
- if(thro_mode == 0.0){
+ if(throwing_mode == 0.0){
if(times >= 5.0 && times < 6.0){
motor_dir1 = 0;
motor_dir2 = 1;
motor_pwm = armpower;
}
- }else if(thro_mode == 1.0){
+ }else if(throwing_mode == 1.0){
if(times >= 5.0 && times < 5.5){
- dpin = 1.0;
+ throwing_relay = 1.0;
}else{
- dpin = 0.0;
+ throwing_relay = 0.0;
}
}
- //wait(1.0);
if(times >= 6.0 && times <7.0){
motor_pwm = 0.0;
motor_dir1 = 0;