ケンタ ミヤザキ
Dependencies: PID Serialservo TCS3200 XQ_servo encoder encoder2 mbed moter
Fork of
nhk_2018_undercarry_test09
by 
ケンタ ミヤザキ
Diff: main.cpp
- Revision:
- 4:eabb7a738ff8
- Parent:
- 3:aaa2fde4fafd
- Child:
- 5:c5a2a5cf600d
--- a/main.cpp Sat Mar 24 10:50:25 2018 +0000
+++ b/main.cpp Wed Mar 28 06:49:41 2018 +0000
@@ -2,22 +2,57 @@
#include "config.h"
#include "moter.h"
#include "PID.h"
+#include "TCS3200.h"
#include "encoder.h"
+#include "encoder2.h"
+#include "Serialservo.h"
+#include "XQ_servo.h"
Serial pc(USBTX, USBRX);
Serial kbtpc(PC_12 ,PD_2);//serial5
I2C master(sda, scl);
-DigitalOut led2(LED2);
+DigitalOut led2(LED2);//チェック用
PID S_pid(SP, SI, SD, 0.0);
PID R_pid(RP, RI, RD, 0.0);
PID L_pid(LP, LI, LD, 0.0);
+
+//DigitalIn zpin(PA_1);
+
Encoder Enc_Carry_Y(PA_15,PA_14,PC_1);
Encoder Enc_Carry_X(PA_4,PB_0,PB_7);
+Encoder2 Enc_Arm(PF_1,PA_0,PA_1);
+
DigitalOut buzzer(PA_10);
DigitalOut lamp(PC_3);
Timer timer;
+Serialservo servo1(PC_10, PC_11);//アームの肘
+Serialservo servo2(PC_10, PC_11);//回転軸
+XQ_servo servo_hand(PB_5);
+//TCS3200 color(PB_5, PB_2, PC_4);
+DigitalOut dpin(PC_9);
+DigitalIn distance(PC_2);
+DigitalIn redIn(PC_4);
+DigitalIn blueIn(PB_2);
+DigitalIn micro_switch(PC_0);
+DigitalOut air(PB_4);
+
+DigitalOut motor_dir1(PA_8); //旧ピン番号
+DigitalOut motor_dir2(PA_9);
+PwmOut motor_pwm(PB_3);
+
+/*
+PwmOut motor_pwm(PA_9);
+DigitalOut motor_dir1(PB_3);
+DigitalOut motor_dir2(PA_8);
+*/
+//DigitalOut motor_relay(PB_10);
+
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 ),
@@ -66,10 +101,32 @@
int state = START;
int times = 0;
bool throwing = false;
+int receive = NOT_RECEIVE;
+int redcount = 0;
+int bluecount = 0;
+int yellowcount = 0;
+bool syokika_flag=false;
+bool set_flag = false;
+int encoder_count = 1000;
+
int main() {
timer.reset();
timer.stop();
- //timer.start();
+ servo1.init(0);//アーム
+ servo2.init(1);//回転軸
+ //servo2.move(50);
+ //servo1.move(152);
+ dpin=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_dir2=1;
+
S_pid.init();
R_pid.init();
L_pid.init();
@@ -77,27 +134,22 @@
pwm_pins[1].period(0.00005);
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) {
- //if(directmode == STRAIGHT){
- master.read(addr1,buff1,2);
- linecheck(buff1,linedata_1);
- //}
- //if(directmode == RIGHT){
- master.read(addr2,buff2,2);
- linecheck(buff2,linedata_2);
- //}
- //if(directmode == LEFT){
- master.read(addr3,buff3,2);
- linecheck(buff3,linedata_3);
- //}
- //master.read(addr1,buff1,2);
- //linecheck(buff1,linedata_1);
- //linecheck(buff1,linedata_1);
- //linecheck(buff1,linedata_1);
+ master.read(addr1,buff1,2);
+ linecheck(buff1,linedata_1);
+ master.read(addr2,buff2,2);
+ linecheck(buff2,linedata_2);
+ master.read(addr3,buff3,2);
+ linecheck(buff3,linedata_3);
x_dist = Enc_Carry_X.read_rotate();
y_dist = Enc_Carry_Y.read_rotate();
kbtpc.putc(state);
@@ -122,6 +174,8 @@
state = MANUAL;
}else if(kbtread == START){
state = START;
+ }else if(kbtread == START_TEST){
+ state = START_TEST;
}
//--------------手動--------------------------------------
if(mode == true){
@@ -140,13 +194,12 @@
//--------------自動---------------------------------
}else if(mode == false){
led2 = 0.0;
- //timer.reset();
- //timer.start();
if(stopflag == true){
Stop(pwm_pins, dir_pins_1, dir_pins_2);
}else{
LineCheck(directmode);
switch(state){
+//------------------任意に進行方向を決めれる----------------------------------------
case MANUAL:
if(crosscount == 1){
directmode = STOP;
@@ -154,8 +207,66 @@
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;
@@ -171,49 +282,71 @@
prelinedata = 0;
}
break;
- //-----------------------------------------------
+//------------------待機位置1----------------------------------------------------
case STAND_BY_1:
crosscount = 0;
directmode = STOP;
- //buzzer = 1.0;
- if(kbtread == RED_RECEIVE){
+ if(distance == 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 >= 100 ){
+ servo_hand.move(hand_tsukami_degree);
+ servo1.move(hiji_tsukami_degree);
+ //pre_touteki();
+ }else if(bluecount >= 100){
+ servo_hand.move(hand_tsukami_degree);
+ servo1.move(hiji_tsukami_degree);
+ //pre_touteki();
+ }
+ }else if(distance == 0.0 && redcount >= 100){
+ servo2.move(koshi_nage_degree);
state = RED_RECEIVE;
- }else if(kbtread == BLUE_RECEIVE){
+ directmode = STOP;
+ redcount = 0;
+ crosscount = 0;
+ }else if(distance == 0.0 && bluecount >= 100){
+ state = BLUE_RECEIVE_1;
+ directmode = STOP;
+ bluecount = 0;
+ crosscount = 0;
+ }
+ /*
+ if(receive == RED_RECEIVE){
+ state = RED_RECEIVE;
+ }else if(receive == BLUE_RECEIVE){
state = BLUE_RECEIVE_1;
}
+ */
break;
+//------------------待機位置1から投てき位置1----------------------------------------
case RED_RECEIVE:
if(crosscount == 0){
directmode = LEFT;
- }else if(crosscount == 1 && throwing == false){
+ }else if(crosscount == 1){
directmode = STOP;
prelinedata = 0;
- if(kbtread == THROWING){
- throwing = true;
- buzzer = 1.0;
- timer.start();
- //lamp = 0.0;
- //wait(3.0);
- //buzzer = 0.0;
- prelinedata = 0;
- //directmode = RIGHT;
- }
- }else if(crosscount == 1 && throwing == true){
- times = timer.read();
- if(times > 2){
- lamp = 0.0;
- buzzer = 0.0;
+ touteki(0.8,190,0);
+ if(set_flag == false){
directmode = RIGHT;
- timer.reset();
- timer.stop();
}
}else if(crosscount == 2){
directmode = STOP;
- state = STAND_BY_1;
- throwing = false;
- crosscount = 0;
+ syokikanomai2();
+ if(set_flag == true){
+ state = STAND_BY_1;
+ //throwing = false;
+ crosscount = 0;
+ }
}
break;
+//------------------待機位置1から投てき位置2----------------------------------------
case BLUE_RECEIVE_1:
if(crosscount == 0){
directmode = RIGHT;
@@ -224,18 +357,44 @@
}else if(crosscount == 3){
directmode = STOP;
prelinedata = 0;
- state = STAND_BY_2;
+ state = BLUE_RECEIVE_2;
crosscount = 0;
}
break;
+//------------------待機位置2----------------------------------------------------
case STAND_BY_2:
crosscount = 0;
+ directmode = STOP;
+ if(distance == 1.0){
+ if(blueIn == 1.0){
+ buzzer = 1.0;
+ bluecount++;
+ }else if(micro_switch == 1.0){
+ buzzer = 1.0;
+ yellowcount++;
+ }else{
+ buzzer = 0.0;
+ }
+ if(bluecount >= 50){
+ pre_touteki();
+ }else if(yellowcount >= 500){
+ air = 1.0;
+ }
+ }else if(distance == 0.0 && bluecount >= 50){
+ state = BLUE_RECEIVE_2;
+ directmode = STOP;
+ bluecount = 0;
+ crosscount = 0;
+ }
+ /*
if(kbtread == BLUE_RECEIVE){
state = BLUE_RECEIVE_2;
}else if(kbtread == YELLOW_RECEIVE){
state = YELLOW_RECEIVE;
}
+ */
break;
+//------------------投てき位置2---------------------------------------------------
case BLUE_RECEIVE_2:
if(crosscount == 0){
directmode = LEFT;
@@ -246,10 +405,11 @@
if(kbtread == THROWING){
throwing = true;
buzzer = 1.0;
- timer.start();
+ //timer.start();
prelinedata = 0;
}
}else{
+ /*
times = timer.read();
if(times > 2){
buzzer = 0.0;
@@ -257,6 +417,7 @@
timer.reset();
timer.stop();
}
+ */
}
}else if(crosscount == 2){
directmode = STOP;
@@ -265,6 +426,7 @@
crosscount = 0;
}
break;
+//------------------投てき位置3---------------------------------------------------
case YELLOW_RECEIVE:
if(crosscount != 4){
directmode = LEFT;
@@ -275,20 +437,12 @@
default:
break;
}
- /*
- if(crosscount == 1){
- directmode = STOP;
- Stop(pwm_pins, dir_pins_1, dir_pins_2);
- crosscount = 0;
- }
- */
+//--------------進行方向---------------------------------------------------------
switch(directmode){
case STOP:
Stop(pwm_pins, dir_pins_1, dir_pins_2);
break;
case STRAIGHT:
- //master.read(addr1,buff1,2);
- //linecheck(buff1, linedata_1);
output = S_pid.compute((double)linedata_1[0]);
if(output >= 0){
rpower = power;
@@ -301,8 +455,6 @@
}
break;
case RIGHT:
- //master.read(addr2,buff2,2);
- //linecheck(buff2, linedata_2);
output = R_pid.compute((double)linedata_2[0]);
if(output >= 0){
rpower = power;
@@ -315,8 +467,6 @@
}
break;
case LEFT:
- //master.read(addr3,buff3,2);
- //linecheck(buff3, linedata_3);
output = L_pid.compute((double)linedata_3[0]);
if(output >= 0){
rpower = power;
@@ -338,11 +488,11 @@
//pc.printf("buff2[0] = %d \r" ,buff2[0]);
//pc.printf("line1 = %d \r" ,linedata_1[0]);
//pc.printf("line2 = %d \r" ,linedata_3[1]);
- //pc.printf("xdist = %f \r",x_dist);
+ pc.printf("xdist = %f \r",x_dist);
//pc.printf("tar_xdist = %f \r",tar_x_dist);
//pc.printf("ontheline = %d \r",ontheline);
- //pc.printf("ydist = %f \r",y_dist);
+ pc.printf("ydist = %f \r",y_dist);
//pc.printf("tary = %f \r",tar_y_dist);
//pc.printf("stopflag = %d \r",stopflag);
//pc.printf("cross = %d \r",crosscount);
@@ -350,11 +500,156 @@
//pc.printf("line = %d" ,line);
//pc.printf("rpower = %lf" ,rpower);
//pc.printf("lpower = %lf" ,lpower);
- pc.printf("dicmode = %d \r",directmode);
+ //pc.printf("dicmode = %d \r",directmode);
pc.printf("output = %lf \n" ,output);
}
}
+//---------------初期化---------------------------
+
+
+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;
+ 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;
+ motor_dir1=1;
+ motor_dir2=1;
+ set_flag=true; //投てき後，falseにする.
+ }
+ }
+ }
+ //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;
+ 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;
+ motor_dir1=1;
+ 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){
+ 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) {
+ motor_dir1 = 0;
+ motor_dir2 = 1;
+ motor_pwm = 0.15;
+ }
+ }
+ motor_dir1=1;
+ motor_dir2=1;
+ motor_pwm=0;
+ //wait(2.0);
+//----投てき-------------------------
+ if(times >= 5.0 && times < 6.0){
+ motor_dir1 = 0;
+ motor_dir2 = 1;
+ motor_pwm = armpower;
+ }
+ //wait(1.0);
+ if(times >= 6.0 && times <7.0){
+ motor_pwm = 0.0;
+ motor_dir1 = 0;
+ motor_dir2 = 0;
+ buzzer = 0;
+ lamp = 1.0;
+ set_flag = false;
+ timer.reset();
+ timer.stop();
+ }
+}
+
void LineCheck(int dmode){
if(dmode == STRAIGHT){
if(linedata_1[1] == 1){