2019NHK_teamA
NHK2019ROBOCON team A manual robot program 10/4 version (ロボコン当日使用プログラム)
Dependencies: mbed 2019ROBOKONmanualProgram
Diff: main.cpp
- Revision:
- 2:830db42bae7e
- Parent:
- 1:99294241f2ba
- Child:
- 3:82008ebeb93c
--- a/main.cpp Thu Sep 05 09:40:50 2019 +0000
+++ b/main.cpp Tue Sep 17 08:54:21 2019 +0000
@@ -9,7 +9,7 @@
0x16:左前
0x18:サーボ制御回路
0x20:右ラック
-0x22:左ラック
+0x22:左ラック.
0x24:回収機構
0x26:右ファン
0x28:左ファン
@@ -30,7 +30,9 @@
DigitalOut pwm(D4);
DigitalOut stop(D10);
DigitalOut led1(LED1);
-
+Timer timer;
+Timer timer2;
+Timer timer3;
//変数
char data1;//右上
@@ -45,7 +47,7 @@
char data9;//左ファン
char data10;//パトランプ
char data11;//吐き出し機構
-char get_data_rs232=0x80;
+char get_data_rs232=0x00;
int Ry;
int Rx;
int Ly;
@@ -78,10 +80,15 @@
int o=1;
int old_right1=0;
int old_left1=0;
-int p=2;
+int p=5;
int q=0;
-
-
+int old_ue1=0;
+int old_shita=0;
+int old_left2=0;
+int old_triangle=0;
+int old_cross=0;
+int r=6;
+int s=5;
//関数プロトタイプ宣言
@@ -90,34 +97,40 @@
void change_data();
void change_pwm();
void change_servo();
-void change_rack();
+//void change_rack();
void change_fan();
void send_data(char address,char data);
void emergency();
void change_rack_2();
-void sequence_kaisyu();
-//void kaisyu();
-void sequence_hakidashi();
-//void hakidashi();
+//void sequence_kaisyu();
+void kaisyu();
+void kaisyu_short();
+//void sequence_hakidashi();
+void hakidashi();
+void mode_change();
//メイン関数
-int main(){
+int main()
+{
led=1;
+ pwm=1;
initialization();
- while(true){
+ while(true) {
emergency();
get_data();
+ mode_change();
change_pwm();
change_servo();
change_data();
- change_rack();
+ //change_rack();
change_rack_2();
- sequence_kaisyu();
- //kaisyu();
- sequence_hakidashi();
- //hakidashi();
+ //sequence_kaisyu();
+ kaisyu();
+ kaisyu_short();
+ //sequence_hakidashi();
+ hakidashi();
change_fan();
send_data(0x10,data1);
send_data(0x12,data2);
@@ -131,16 +144,17 @@
send_data(0x28,data9);
send_data(0x40,data10);
//send_data(0x30,data11);
-
+
+
+
-
-
- }
}
-
-
+}
+
+
//初期化
-void initialization(){
+void initialization()
+{
data1=0x80;
data2=0x80;
data3=0x80;
@@ -152,7 +166,7 @@
data9=0x80;
data10=0x80;
data11=0x80;
-
+
send_data(0x10,data1);
send_data(0x12,data2);
send_data(0x14,data3);
@@ -165,12 +179,26 @@
send_data(0x40,data10);
send_data(0x30,data11);
-
- }
+ data10=0x3f;
+ send_data(0x40,data10);
+ if(q==0){
+ data10=0xdf;
+ send_data(0x40,data10);
+ }else if(q==1){
+ data10=0xcf;
+ send_data(0x40,data10);
+ }
+
+ timer.reset();
+
+}
+
+
//データ読み込み
-void get_data(){
+void get_data()
+{
Ry=ps3.getRightJoystickYaxis();
Rx=ps3.getRightJoystickXaxis();
Ly=ps3.getLeftJoystickYaxis();
@@ -189,241 +217,298 @@
hidari1=ps3.getButtonState(hidari);
migi1=ps3.getButtonState(migi);
get_data_rs232=slave.getc();
-
+
- if(get_data_rs232==0x20||get_data_rs232==0x22||get_data_rs232==0x24||get_data_rs232==0x32||get_data_rs232==0x34){
+ if(get_data_rs232) {
led1=1;
- }else{
- led1=0;
- }
- pc.printf("%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n",Ry,Rx,Ly,left1,left2,right1,right2,select,start,ue1,shita,triangle,square,migi1,hidari1,tushin_check,i);
- if(Ry==0&&Rx==0&&Ly==0&&left1==0&&right1==0&&right2==0&&left2==0&&select==0&&start==0&&shita==0&&triangle==0&&ue1==0&&migi1==0&&hidari1==0&&square==0&&circle==0&&cross==0){
+ } else {
+ led1=0;
+ }
+ //pc.printf("%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n",Ry,Rx,Ly,left1,left2,right1,right2,select,start,ue1,shita,triangle,square,migi1,hidari1,tushin_check,i);
+ if(Ry==0&&Rx==0&&Ly==0&&left1==0&&right1==0&&right2==0&&left2==0&&select==0&&start==0&&shita==0&&triangle==0&&ue1==0&&migi1==0&&hidari1==0&&square==0&&circle==0&&cross==0) {
data_check=0;
- }else{
- data_check=1;
- }
-
+ } else {
+ data_check=1;
}
-
+
+}
+
//緊急停止
-void emergency(){
- if(start!=old_start){
- old_start=start;
- if(start==1){
- if(j==1){
- //緊急停止
- led=0;
- stop=1;
- j=0;
- }else if(j==0){
- //緊急停止解除
- led=1;
- stop=0;
- j=1;
+void emergency()
+{
+ if(start!=old_start) {
+ old_start=start;
+ if(start==1) {
+ if(j==1) {
+ //緊急停止
+ led=0;
+ stop=1;
+ j=0;
+ } else if(j==0) {
+ //緊急停止解除
+ led=1;
+ stop=0;
+ j=1;
}
}
- }
+ }
+}
+
+
+//モードチェンジ
+void mode_change(){
+ if(old_select!=select){
+ old_select=select;
+ if(select==1){
+ if(q==0){
+ //送風モード
+ q=1;
+ data10=0xcf;
+ send_data(0x40,data10);
+ }else if(q==1){
+ //回収モード
+ q=0;
+ data10=0xdf;
+ send_data(0x40,data10);
+ }
+ }
+ }
}
-
//データ変化(メカナム)
-void change_data(){
-
- if(Ry!=0||Rx!=0){
+void change_data()
+{
+
+ if(Ry!=0||Rx!=0) {
square=0;
+ migi1=0;
+ }
+
+ if(Ry>30&&i==0) {
+ if(right1==1) {
+ data1=0xcf;
+ data2=0x00;
+ data3=0xcf;
+ data4=0x00;
+ } else if(left1==1) {
+ data1=0x00;
+ data2=0xcf;
+ data3=0x00;
+ data4=0xcf;
+ } else {
+ data1=0x00;
+ data2=0x00;
+ data3=0x00;
+ data4=0x00;
}
-
- if(Ry>30&&i==0){
+ } else if(Ry<-30&&i==0) {
if(right1==1){
- data1=0xcf;
- data2=0x00;
- data3=0xcf;
- data4=0x00;
+ data1=0xdf;
+ data2=0xff;
+ data3=0xdf;
+ data4=0xff;
}else if(left1==1){
- data1=0x00;
- data2=0xcf;
- data3=0x00;
- data4=0xcf;
- }else{
- data1=0x00;
- data2=0x00;
- data3=0x00;
- data4=0x00;
- }
- }else if(Ry<-30&&i==0){
- data1=0xff;
- data2=0xff;
- data3=0xff;
- data4=0xff;
- }else if(Rx>30&&i==0){
- if(right1==1){
data1=0xff;
data2=0xdf;
- data3=0xcf;
- data4=0x00;
- }else if(left1==1){
- data1=0xdf;
- data2=0xff;
- data3=0x00;
- data4=0xcf;
- }else{
- data1=0xff;
- data2=0xff;
- data3=0x00;
- data4=0x00;
- }
- }else if(Rx<-30&&i==0){
- if(right1==1){
- data1=0xcf;
- data2=0x00;
data3=0xff;
data4=0xdf;
- }else if(left1==1){
- data1=0x00;
- data2=0xcf;
- data3=0xdf;
+ }else{
+ data1=0xff;
+ data2=0xff;
+ data3=0xff;
data4=0xff;
- }else{
- data1=0x00;
- data2=0x00;
- data3=0xff;
- data4=0xff;
+ }
+ } else if(Rx>30&&i==0) {
+ if(right1==1) {
+ data1=0xff;
+ data2=0xdf;
+ data3=0xcf;
+ data4=0x00;
+ } else if(left1==1) {
+ data1=0xdf;
+ data2=0xff;
+ data3=0x00;
+ data4=0xcf;
+ } else {
+ data1=0xff;
+ data2=0xff;
+ data3=0x00;
+ data4=0x00;
}
- }else if(right1==1&&i==0){
+ } else if(Rx<-30&&i==0) {
+ if(right1==1) {
+ data1=0xcf;
+ data2=0x00;
+ data3=0xff;
+ data4=0xdf;
+ } else if(left1==1) {
+ data1=0x00;
+ data2=0xcf;
+ data3=0xdf;
+ data4=0xff;
+ } else {
+ data1=0x00;
+ data2=0x00;
+ data3=0xff;
+ data4=0xff;
+ }
+ } else if(right1==1&&i==0) {
data1=0xff;
data2=0x00;
data3=0xff;
data4=0x00;
- }else if(left1==1&&i==0){
+ } else if(left1==1&&i==0) {
data1=0x00;
data2=0xff;
data3=0x00;
data4=0xff;
- }else if(Ry>30&&i==1){
- if(right1==1){
- data1=0x4f;
- data2=0x3f;
- data3=0x4f;
- data4=0x3f;
- }else if(left1==1){
- data1=0x3f;
- data2=0x4f;
- data3=0x3f;
- data4=0x4f;
- }else{
- data1=0x3f;
- data2=0x3f;
- data3=0x3f;
- data4=0x3f;
+ } else if(Ry>30&&i==1) {
+ if(right1==1) {
+ data1=0x4f;
+ data2=0x3f;
+ data3=0x4f;
+ data4=0x3f;
+ } else if(left1==1) {
+ data1=0x3f;
+ data2=0x4f;
+ data3=0x3f;
+ data4=0x4f;
+ } else {
+ data1=0x3f;
+ data2=0x3f;
+ data3=0x3f;
+ data4=0x3f;
+ }
+ } else if(Ry<-30&&i==1) {
+ if(right1==1){
+ data1=0x5f;
+ data2=0xbf;
+ data3=0x5f;
+ data4=0xbf;
+ }else if(left1==1){
+ data1=0xbf;
+ data2=0x5f;
+ data3=0xbf;
+ data4=0x5f;
+ }else{
+ data1=0xbf;
+ data2=0xbf;
+ data3=0xbf;
+ data4=0xbf;
}
- }else if(Ry<-30&&i==1){
- data1=0xbf;
- data2=0xbf;
- data3=0xbf;
- data4=0xbf;
- }else if(Rx>30&&i==1){
- if(right1==1){
- data1=0xbf;
- data2=0x5f;
- data3=0x4f;
- data4=0x3f;
- }else if(left1==1){
- data1=0x5f;
- data2=0xbf;
- data3=0x3f;
- data4=0x4f;
- }else{
- data1=0xbf;
- data2=0xbf;
- data3=0x3f;
- data4=0x3f;
+ } else if(Rx>30&&i==1) {
+ if(right1==1) {
+ data1=0xbf;
+ data2=0x5f;
+ data3=0x4f;
+ data4=0x3f;
+ } else if(left1==1) {
+ data1=0x5f;
+ data2=0xbf;
+ data3=0x3f;
+ data4=0x4f;
+ } else {
+ data1=0xbf;
+ data2=0xbf;
+ data3=0x3f;
+ data4=0x3f;
}
- }else if(Rx<-30&&i==1){
- if(right1==1){
- data1=0x4f;
- data2=0x3f;
- data3=0xbf;
- data4=0x5f;
- }else if(left1==1){
- data1=0x3f;
- data2=0x4f;
- data3=0x5f;
- data4=0xbf;
- }else{
- data1=0x3f;
- data2=0x3f;
- data3=0xbf;
- data4=0xbf;
+ } else if(Rx<-30&&i==1) {
+ if(right1==1) {
+ data1=0x4f;
+ data2=0x3f;
+ data3=0xbf;
+ data4=0x5f;
+ } else if(left1==1) {
+ data1=0x3f;
+ data2=0x4f;
+ data3=0x5f;
+ data4=0xbf;
+ } else {
+ data1=0x3f;
+ data2=0x3f;
+ data3=0xbf;
+ data4=0xbf;
}
- }else if(right1==1&&i==1){
+ } else if(right1==1&&i==1) {
data1=0xbf;
data2=0x3f;
data3=0xbf;
data4=0x3f;
- }else if(left1==1&&i==1){
+ } else if(left1==1&&i==1) {
data1=0x3f;
data2=0xbf;
data3=0x3f;
data4=0xbf;
- }else{
+ } else {
data1=0x80;
data2=0x80;
data3=0x80;
data4=0x80;
- }
}
+}
-//pwm変化
-void change_pwm(){
-if(select!=old_select){
- old_select=select;
- if(select==1){
- if(i==1){
- pwm=0;
- i=0;
- }else if(i==0){
- pwm=1;
- i=1;
+//pwm変化
+void change_pwm()
+{
+ if(cross!=old_cross) {
+ old_cross=cross;
+ if(cross==1) {
+ if(i==1) {
+ pwm=0;
+ data10=0xbf;
+ send_data(0x40,data10);
+ i=0;
+ } else if(i==0) {
+ pwm=1;
+ data10=0x3f;
+ send_data(0x40,data10);
+ i=1;
}
}
}
}
//サーボモーター
-void change_servo(){
- if(right2!=old_right2){
- old_right2=right2;
- //ハンガー機構
- if(right2==1){
- if(o==1){
- data_servo=0x01;
- //send_data(0x18,data_servo);
- o=2;
- }else if(o==2){
- data_servo=0x02;
- //send_data(0x18,data_servo);
- o=3;
- }
- //data_servo=0x01;
- }
- }else if(circle!=old_circle){
- old_circle=circle;
- //バスタオル挟む機構
- if(circle==1){
- if(k==1){
- data_servo=0x03;
- //send_data(0x18,data_servo);
- k=0;
- }else if(k==0){
- data_servo=0x04;
- //send_data(0x18,data_servo);
- k=1;
- }
- }
- }/*else if(hidari1!=old_hidari1){
+void change_servo()
+{
+ if(right2!=old_right2) {
+ old_right2=right2;
+ //ハンガー機構&バスタオル機構initialize処理
+ if(right2==1) {
+ if(o==1) {
+
+ // timer.start();
+ // data_servo=0x03;
+ // send_data(0x18,data_servo);
+ data_servo=0x01;
+ //send_data(0x18,data_servo);
+ o=2;
+ } else if(o==2) {
+ data_servo=0x02;
+ //send_data(0x18,data_servo);
+ o=3;
+ }
+ //data_servo=0x01;
+ }
+ } else if(circle!=old_circle) {
+ old_circle=circle;
+ //バスタオル挟む機構
+ if(circle==1) {
+ if(k==1) {
+ data_servo=0x03;
+ //send_data(0x18,data_servo);
+ k=0;
+ } else if(k==0) {
+ data_servo=0x04;
+ //send_data(0x18,data_servo);
+ k=1;
+ }
+ }
+ }/*else if(hidari1!=old_hidari1){
old_hidari1=hidari1;
-
+
if(hidari1==1&&select==0){
if(n==1){
data_servo=0x05;
@@ -434,243 +519,443 @@
}
}
}*/
- }
-
+
+ /* if(timer.read()>2&&o==2){
+ data_servo=0x04;
+ send_data(0x18,data_servo);
+ timer.stop();
+ timer.reset();
+ }*/
+}
+
//ラック
-void change_rack(){
-
-
- //右
- if(triangle==1&&cross==0&&ue1==0&&shita==0&&(-32<Ly<32)){
- if(get_data_rs232!=0x20&&get_data_rs232!=0x22){
- data5=0x00;
- }else if(get_data_rs232==0x20||get_data_rs232==0x22){
- data5=0x10;
- }
- }else if(triangle==0&&cross==1&&ue1==0&&shita==0&&(-32<Ly<32)){
- if(get_data_rs232!=0x26&&get_data_rs232!=0x28){
- data5=0xff;
- }else if(get_data_rs232==0x26||get_data_rs232==0x28){
- data5=0x10;
- }
+/*
+void change_rack()
+{
+
+
+ //右
+ if(triangle==1&&cross==0&&ue1==0&&shita==0&&(-32<Ly<32)) {
+ if(!(get_data_rs232 & 0x01)) {
+ data5=0x00;
+ } else if(get_data_rs232 & 0x01) {
+ data5=0x10;
+ }
+ } else if(triangle==0&&cross==1&&ue1==0&&shita==0&&(-32<Ly<32)) {
+ if(!(get_data_rs232 & 0x04)) {
+ data5=0xff;
+ } else if(get_data_rs232 & 0x04) {
+ data5=0x10;
+ }
//左
- }else if(triangle==0&&cross==0&&ue1==1&&shita==0&&(-32<Ly<32)){
- if(get_data_rs232!=0x20&&get_data_rs232!=0x24){
- data6=0xff;
- }else if(get_data_rs232==0x20||get_data_rs232==0x24){
- data6=0x10;
- }
- }else if(triangle==0&&cross==0&&ue1==0&&shita==1&&(-32<Ly<32)){
- if(get_data_rs232!=0x26&&get_data_rs232!=0x30){
- data6=0x00;
- }else if(get_data_rs232==0x26||get_data_rs232==0x30){
- data6=0x10;
- }
- }else if(triangle==0&&cross==0&&ue1==0&&shita==0&&(-32<Ly<32)){
- data5=0x80;
- data6=0x80;
-
+ } else if(triangle==0&&cross==0&&ue1==1&&shita==0&&(-32<Ly<32)) {
+ if(!(get_data_rs232 & 0x02)) {
+ data6=0xff;
+ } else if(get_data_rs232 & 0x02) {
+ data6=0x10;
}
+ } else if(triangle==0&&cross==0&&ue1==0&&shita==1&&(-32<Ly<32)) {
+ if(!(get_data_rs232 & 0x08)) {
+ data6=0x00;
+ } else if(get_data_rs232 & 0x08) {
+ data6=0x10;
+ }
+ } else if(triangle==0&&cross==0&&ue1==0&&shita==0&&(-32<Ly<32)) {
+ data5=0x80;
+ data6=0x80;
+
+ }
}
+*/
//ラック左右
-void change_rack_2(){
- if(triangle==0&&cross==0&&ue1==0&&shita==0&&Ly>32){
- if(get_data_rs232==0x20){
+void change_rack_2()
+{
+ /*
+ if(triangle==0&&cross==0&&ue1==0&&shita==0&&Ly>32) {
+ if(get_data_rs232 & 0x01) {
data5=0x10;
+ } else {
+ data5=0x00;
+ }
+ if(get_data_rs232 & 0x02) {
data6=0x10;
- }else if(get_data_rs232==0x22){
- data5=0x10;
+ } else {
data6=0xff;
- }else if(get_data_rs232==0x24){
- data5=0x00;
+ }
+ } else if(triangle==0&&cross==0&&ue1==0&&shita==0&&Ly<-32) {
+ if(get_data_rs232 & 0x04) {
+ data5=0x10;
+ } else {
+ data5=0xff;
+ }
+ if(get_data_rs232 & 0x08) {
data6=0x10;
- }else{
+ } else {
+ data6=0x00;
+ }
+ } else if(triangle==0&&cross==0&&ue1==0&&shita==0&&(-32<Ly<32)) {
+ data5=0x80;
+ data6=0x80;
+ }
+ */
+
+ if(ue1!=old_ue1){
+ old_ue1=ue1;
+ if(ue1==1&&start==0){
data5=0x00;
data6=0xff;
}
- }else if(triangle==0&&cross==0&&ue1==0&&shita==0&&Ly<-32){
- if(get_data_rs232==0x26){
- data5=0x10;
- data6=0x10;
- }else if(get_data_rs232==0x28){
- data5=0x10;
- data6=0x00;
- }else if(get_data_rs232==0x30){
- data5=0xff;
- data6=0x10;
- }else{
+ }else if(shita!=old_shita){
+ old_shita=shita;
+ if(shita==1&&start==0){
data5=0xff;
data6=0x00;
}
- }else if(triangle==0&&cross==0&&ue1==0&&shita==0&&(-32<Ly<32)){
- data5=0x80;
- data6=0x80;
+ }
+
+ if((get_data_rs232 & 0x01)&&data5==0x00){
+ data5=0x10;
+ send_data(0x20,data5);
+ }else if((get_data_rs232 & 0x04)&&data5==0xff){
+ data5=0x10;
+ send_data(0x20,data5);
}
+
+ if((get_data_rs232 & 0x02)&&data6==0xff){
+ data6=0x10;
+ send_data(0x22,data6);
+ }else if((get_data_rs232 & 0x08)&&data6==0x00){
+ data6=0x10;
+ send_data(0x22,data6);
+ }
+
+
}
//回収機構
+/*
+void sequence_kaisyu()
+{
+ if(q==0){
+ if(square!=old_square) {
+ old_square=square;
+ if(square==1) {
-void sequence_kaisyu(){
- if(square!=old_square){
- old_square=square;
- if(square==1){
-
- data_servo=0x07;
- send_data(0x18,data_servo);
- wait(1.5);
- data7=0x00;
- while(get_data_rs232!=0x32){
- send_data(0x24,data7);
- get_data_rs232=slave.getc();
- }
- data7=0x10;
- send_data(0x24,data7);
- data_servo=0x08;
- send_data(0x18,data_servo);
- wait(2);
- data7=0xff;
- while(get_data_rs232!=0x34){
- send_data(0x24,data7);
- get_data_rs232=slave.getc();
- }
- data7=0x10;
- send_data(0x24,data7);
- data_servo=0x07;
- send_data(0x18,data_servo);
+ data_servo=0x07;
+ send_data(0x18,data_servo);
+ wait(1.5);
+ data7=0x00;
+ while(!(get_data_rs232 & 0x10)) {
+ send_data(0x24,data7);
+ get_data_rs232=slave.getc();
+ }
+ data7=0x10;
+ send_data(0x24,data7);
+ data_servo=0x08;
+ send_data(0x18,data_servo);
+ wait(1.5);
+ data7=0xff;
+ while(!(get_data_rs232 & 0x20)) {
+ send_data(0x24,data7);
+ get_data_rs232=slave.getc();
+ }
+ data7=0x10;
+ send_data(0x24,data7);
+
+ }
+ }
+ }
+}
+*/
+
+
+void kaisyu(){
+ if(q==0){
+ if(old_square!=square){
+ old_square=square;
+ if(square==1){
+ p=1;
+
+ }
+ }
+ switch(p){
+
+ case 1:
+ data_servo=0x07;
+ send_data(0x24,data_servo);
+ timer2.start();
+ if(timer2.read()>3.5){
+ timer2.stop();
+ timer2.reset();
+ p=2;
+ }
+ break;
+
+ case 2:
+ data7=0x00;
+ send_data(0x24,data7);
+ if(get_data_rs232 & 0x10){
+ data7=0x10;
+ send_data(0x24,data7);
+ p=3;
+ }
+ break;
+
+ case 3:
+ data_servo=0x08;
+ send_data(0x18,data_servo);
+ timer2.start();
+ if(timer2.read()>1.3){
+ p=4;
+ }
+
+ break;
+
+ case 4:
+ data7=0xff;
+ send_data(0x24,data7);
+ if(get_data_rs232 & 0x20){
+ data7=0x10;
+ send_data(0x24,data7);
+ p=5;
+ }
+ break;
+
+ default:
+ p=5;
+ break;
+
+ }
+ }
+ }
+
+
+/*
+void sequence_hakidashi()
+{
+ if(q==0){
+ if(migi1!=old_migi1) {
+ old_migi1=migi1;
+ if(migi1==1&&select==0) {
+ data_servo=0x07;
+ send_data(0x18,data_servo);
+ wait(1);
+ data7=0x00;
+ send_data(0x24,data7);
+ wait(0.5);
+ data7=0x10;
+ send_data(0x24,data7);
+ data11=0xff;
+ while(!(get_data_rs232 & 0x40)){
+ send_data(0x30,data11);
+ get_data_rs232=slave.getc();
+ }
+ data11=0x10;
+ send_data(0x38,data11);
+ data11=0x00;
+ while(!(get_data_rs232 & 0x80)){
+ send_data(0x30,data11);
+ get_data_rs232=slave.getc();
+ }
+ data11=0x10;
+ send_data(0x30,data11);
+ data7=0xff;
+ while(!(get_data_rs232 & 0x20)){
+ send_data(0x24,data7);
+ get_data_rs232=slave.getc();
+ }
+ data7=0x10;
+ send_data(0x24,data7);
+
+ }
+ }
+ }
+}
+*/
+
- }
- }
+void hakidashi(){
+ if(q==0){
+ if(old_migi1!=migi1){
+ old_migi1=migi1;
+ if(migi1==1){
+ r=1;
+ }
+ }
+
+ switch(r){
+ case 1:
+ data_servo=0x07;
+ send_data(0x18,data_servo);
+ timer2.start();
+ if(timer2.read()>1.5){
+ timer2.stop();
+ timer2.reset();
+ r=2;
+ }
+ break;
+
+ case 2:
+ data7=0x00;
+ send_data(0x24,data7);
+ timer2.start();
+ if(timer2.read()>0.3){
+ timer2.stop();
+ timer2.reset();
+ data7=0x10;
+ send_data(0x24,data7);
+ r=3;
+ }
+ break;
+
+ case 3:
+ data11=0xff;
+ send_data(0x30,data11);
+ if(get_data_rs232 & 0x40){
+ data11=0x10;
+ send_data(0x30,data11);
+ r=4;
+ }
+ break;
+
+ case 4:
+ data11=0x00;
+ send_data(0x30,data11);
+ if(get_data_rs232 & 0x80){
+ data11=0x10;
+ send_data(0x30,data11);
+ r=5;
+ }
+ break;
+
+ case 5:
+ data7=0xff;
+ send_data(0x24,data7);
+ if(get_data_rs232 & 0x20){
+ data7=0x10;
+ send_data(0x24,data7);
+ r=6;
+ }
+ break;
+
+ default:
+ r=6;
+ break;
+ }
+ }
+}
+
+void kaisyu_short(){
+ if(q==0){
+ if(triangle!=old_triangle){
+ old_triangle=triangle;
+ if(triangle==1){
+ s=1;
+ }
+ }
+
+ switch(s){
+ case 1:
+ data_servo=0x07;
+ send_data(0x18,data_servo);
+ timer3.start();
+ if(timer3.read()>3.5){
+ timer3.stop();
+ timer3.reset();
+ s=2;
+ }
+ break;
+
+ case 2:
+ data7=0x00;
+ send_data(0x24,data7);
+ timer3.start();
+ if(timer3.read()>0.5){
+ data7=0x10;
+ send_data(0x24,data7);
+ s=3;
+ }
+ break;
+
+ case 3:
+ data_servo=0x08;
+ send_data(0x18,data_servo);
+ timer3.start();
+ if(timer3.read()>1.3){
+ s=4;
+ }
+ break;
+
+ case 4:
+ data7=0xff;
+ send_data(0x24,data7);
+ if(get_data_rs232 & 0x20){
+ data7=0x10;
+ send_data(0x24,data7);
+ s=5;
+ }
+ break;
+
+ default:
+ s=5;
+ break;
+ }
+ }
+}
+
+//ファン
+void change_fan()
+{
+ if(q==1){
+ if(left2==1&&(old_triangle==triangle)) {
+ data8=0xff;
+ data9=0xff;
+ data10=0xff;
+ } else if(left2==0&&(old_triangle==triangle)) {
+ data8=0x00;
+ data9=0x00;
+ data10=0x00;
+ }else if(left2==0&&(old_triangle!=triangle)&&select==0){
+ old_triangle=triangle;
+ if(triangle==1){
+ data8=0xff;
+ data9=0xff;
+ data10=0xff;
+ data_servo=0x03;
+ send_data(0x26,data8);
+ send_data(0x28,data9);
+ send_data(0x40,data10);
+ wait(0.5);
+ send_data(0x18,data_servo);
+ wait(1);
+ data8=0x00;
+ data9=0x00;
+ data10=0x00;
+ send_data(0x26,data8);
+ send_data(0x28,data9);
+ send_data(0x40,data10);
+ wait(0.5);
+ data_servo=0x04;
+ send_data(0x18,data_servo);
+ }
+ }
+}
+
}
-/*void kaisyu(){
- if(square!=old_square){
- if(square==1){
- p=1;
- data_servo=0x07;
- send_data(0x18,data_servo);
- wait(1.5);
- }
- }
-
- //get_data_rs232=slave.getc();
-
- if(p==1&&get_data_rs232_2!=0x32){
- data7=0x00;
- }else if(p==1&&get_data_rs232_2==0x32){
- data7=0x10;
- p=0;
- data_servo=0x08;
- send_data(0x18,data_servo);
- wait(1.5);
- }else if(p==0&&get_data_rs232!=0x34){
- data7=0xff;
- }else if(p==0&&get_data_rs232==0x34){
- data7=0x10;
- p=2;
- }
-
- }
-*/
-
-void sequence_hakidashi(){
- if(migi1!=old_migi1){
- old_migi1=migi1;
- if(migi1==1){
- /* data_servo=0x07;
- send_data(0x18,data_servo);
- wait(1.5);
- data11=0xff;
- while(get_data_rs232!=0x36){
- send_data(0x30,data11);
- get_data_rs232=slave.getc();
- }
- data11=0x10;
- send_data(0x30,data11);*/
- data11=0xff;
- send_data(0x30,data11);
- }
- }else if(hidari1!=old_hidari1){
- if(hidari1==1){
- data11=0x00;
- send_data(0x30,data11);
- }
- } /* else if(hidari1==1){
- if(get_data_rs232!=0x38){
- data11=0x38;
- send_data(0x30,data11);
- }else if(get_data_rs232==0x38){
- data11=0x10;
- send_data(0x30,data11;
- }
- }*/ /*else{
- data11=0x80;
- send_data(0x30,data11);
- }*/
-
- if(get_data_rs232==0x38&&data11==0x00){
- data11=0x10;
- send_data(0x30,data11);
- } else if(get_data_rs232==0x36&&data11==0xff){
- data11=0x10;
- send_data(0x30,data11);
-
- }
- }
-
-
-/*
-void hakidashi(){
- if(migi1==1&&hidari1==0){
- data_servo=0x07;
- if(get_data_rs232==0x36){
- data11=0x10;
- }else if(get_data_rs232!=0x38){
- data11=0xff;
- }
- }else if(migi1==0&&hidari1==1){
- data_servo=0x08;
- if(get_data_rs232==0x38){
- data11=0x10;
- }else if(get_data_rs232!=0x38){
- data11=0x00;
- }
- }else{
- data11=0x80;
- }
-}
-
-*/
-
-//ファン
-void change_fan(){
- if(left2==1){
- data8=0xff;
- data9=0xff;
- data10=0xff;
- }else if(left2==0){
- data8=0x00;
- data9=0x00;
- data10=0x00;
- }
- }
-
-
-
-
//i2c
-void send_data(char address,char data){
+void send_data(char address,char data)
+{
tsushin=1;
tushin_check=0;
- i2c.frequency(100000);
i2c.start();
i2c.write(address);
tushin_check=i2c.write(data);
i2c.stop();
- wait(0.003);
- }
-
+}
+