2019NHK_teamA
A team manual robot program 10/3 もとどうり
Diff: main.cpp
- Revision:
- 2:283e5bb8a712
- Parent:
- 0:f99e0ea93e31
--- a/main.cpp Wed Aug 28 09:05:52 2019 +0000
+++ b/main.cpp Thu Oct 03 11:58:43 2019 +0000
@@ -4,12 +4,12 @@
/*
アドレス
0x10:右前
-0x12:左前
+0x12:左後ろ
0x14:右後ろ
-0x16:左後ろ
+0x16:左前
0x18:サーボ制御回路
0x20:右ラック
-0x22:左ラック
+0x22:左ラック.
0x24:回収機構
0x26:右ファン
0x28:左ファン
@@ -30,6 +30,8 @@
DigitalOut pwm(D4);
DigitalOut stop(D10);
DigitalOut led1(LED1);
+Timer timer;
+Timer timer2;
//変数
char data1;//右上
@@ -44,7 +46,7 @@
char data9;//左ファン
char data10;//パトランプ
char data11;//吐き出し機構
-char get_data_rs232=0x80;
+char get_data_rs232=0x00;
int Ry;
int Rx;
int Ly;
@@ -77,40 +79,57 @@
int o=1;
int old_right1=0;
int old_left1=0;
-
-
-
+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;
//関数プロトタイプ宣言
+void initialization();
void get_data();
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_kaisyu_short();
//void sequence_hakidashi();
-//void hakidashi();
+void hakidashi();
+void mode_change();
+
+
//メイン関数
-int main(){
+int main()
+{
led=1;
- data7=0x80;
- while(true){
+ pwm=1;
+ initialization();
+ while(true) {
emergency();
get_data();
+ mode_change();
change_pwm();
change_servo();
change_data();
- change_rack();
+ //change_rack();
change_rack_2();
sequence_kaisyu();
- //sequence_hakidashi();
- //hakidashi();
+ //kaisyu();
+ //sequence_kaisyu_short();
+ //sequence_hakidashi();
+ hakidashi();
change_fan();
send_data(0x10,data1);
send_data(0x12,data2);
@@ -119,15 +138,70 @@
send_data(0x18,data_servo);
send_data(0x20,data5);
send_data(0x22,data6);
+ //send_data(0x24,data7);
send_data(0x26,data8);
send_data(0x28,data9);
send_data(0x40,data10);
-
- }
+ //send_data(0x30,data11);
+
+
+
+
+ }
+}
+
+
+//初期化
+void initialization()
+{
+ data1=0x80;
+ data2=0x80;
+ data3=0x80;
+ data4=0x80;
+ data5=0x80;
+ data6=0x80;
+ data7=0x10;
+ data8=0x80;
+ data9=0x80;
+ data10=0x80;
+ data11=0x80;
+
+ send_data(0x10,data1);
+ send_data(0x12,data2);
+ send_data(0x14,data3);
+ send_data(0x16,data4);
+ send_data(0x20,data5);
+ send_data(0x22,data6);
+ send_data(0x24,data7);
+ send_data(0x26,data8);
+ send_data(0x28,data9);
+ 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);
}
+ /*data_servo=0x03;
+ send_data(0x18,data_servo);
+ data_servo=0x04;
+ send_data(0x18,data_servo);*/
+ timer2.reset();
+
+}
+
+
//データ読み込み
-void get_data(){
+void get_data()
+{
Ry=ps3.getRightJoystickYaxis();
Rx=ps3.getRightJoystickXaxis();
Ly=ps3.getLeftJoystickYaxis();
@@ -146,181 +220,303 @@
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;
+ /* data_servo=0x03;
+ send_data(0x18,data_servo);
+ data_servo=0x04;
+ send_data(0x18,data_servo);*/
+ data10=0xcf;
+ send_data(0x40,data10);
+ }else if(q==1){
+ //回収モード
+ q=0;
+ data10=0xdf;
+ send_data(0x40,data10);
+ }
+ }
+ }
}
-
//データ変化(メカナム)
-void change_data(){
- if(Ry>30&&i==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;
+ }
+ } 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;
+ data1=0xff;
+ data2=0xdf;
+ data3=0xff;
+ data4=0xdf;
}else{
- data1=0x00;
- data2=0x00;
- data3=0x00;
- data4=0x00;
+ data1=0xff;
+ data2=0xff;
+ data3=0xff;
+ data4=0xff;
}
- }else if(Ry<-30&&i==0){
- data1=0xff;
- data2=0xff;
- data3=0xff;
- data4=0xff;
- }else if(Rx>30&&i==0){
- data1=0x00;
- data2=0xff;
- data3=0xff;
- data4=0x00;
- }else if(Rx<-30&&i==0){
- data1=0xff;
- data2=0x00;
- data3=0x00;
- data4=0xff;
- }else if(right1==1&&i==0){
+ } 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;
+ 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){
- data1=0x3f;
- data2=0xbf;
- data3=0xbf;
- data4=0x3f;
- }else if(Rx<-30&&i==1){
- data1=0xbf;
- data2=0x3f;
- data3=0x3f;
- data4=0xbf;
- }else if(right1==1&&i==1){
+ } 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(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;
- o=0;
- }else if(o==0){
- data_servo=0x02;
- o=1;
- }
- }
- }else if(circle!=old_circle){
- old_circle=circle;
- //バスタオル挟む機構
- if(circle==1){
- if(k==1){
- data_servo=0x03;
- k=0;
- }else if(k==0){
- data_servo=0x04;
- 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;
@@ -331,204 +527,445 @@
}
}
}*/
- }
-
+
+ /* 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;
+
+ /*data_servo=0x03;
+ send_data(0x18,data_servo);
+ data_servo=0x04;
+ send_data(0x18,data_servo);*/
}
- }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(square!=old_square){
- old_square=square;
- if(square==1){
- /*data_servo=0x07;
- send_data(0x18,data_servo);
- wait(2);
- data7=0xff;
- send_data(0x24,data7);
- wait(4);
- data7=0x10;
- send_data(0x24,data7);
- data_servo=0x08;
- send_data(0x18,data_servo);
- wait(2);
- data7=0x00;
- send_data(0x24,data7);
- wait(4);
- data7=0x10;
- send_data(0x24,data7);*/
-
- 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);
+
+void sequence_kaisyu()
+{
+ if(q==0){
+ if(square!=old_square) {
+ old_square=square;
+ if(square==1&&select==0) {
+
+ data_servo=0x07;
+ send_data(0x18,data_servo);
+ wait(1.3);
+ data7=0x00;
+ get_data_rs232=get_data_rs232 & 0b11101111;
+ 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.3);
+ data7=0xff;
+ get_data_rs232=get_data_rs232 & 0b11011111;
+ while(!(get_data_rs232 & 0x20)) {
+ send_data(0x24,data7);
+ get_data_rs232=slave.getc();
+ }
+ data7=0x10;
+ send_data(0x24,data7);
+
+ }
+ }else if(triangle!=old_triangle){
+ if(triangle==1&&select==0){
+
+ data_servo=0x07;
+ send_data(0x18,data_servo);
+ wait(1.3);
+ data7=0x00;
+ send_data(0x24,data7);
+ wait(0.3);
+ data7=0x10;
+ send_data(0x24,data7);
+ data_servo=0x08;
+ send_data(0x18,data_servo);
+ wait(1.3);
+ data7=0xff;
+ get_data_rs232=get_data_rs232 & 0b11011111;
+ 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(migi1!=old_migi1){
- old_migi1=migi1;
- if(migi1==1){
- data_servo=0x07;
- send_data(0x18,data_servo);
- wait(2);
- data11=0xff;
- while(get_data_rs232!=0x36){
- send_data(0x30,data11);
- get_data_rs232=slave.getc();
- }
+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);
- wait(0.5);
- data11=0x00;
- while(get_data_rs232!=0x38){
- send_data(0x30,data11);
+ data7=0xff;
+ while(!(get_data_rs232 & 0x20)){
+ send_data(0x24,data7);
get_data_rs232=slave.getc();
- }
- data11=0x10;
- send_data(0x30,data11);
- data_servo=0x08;
- send_data(0x18,data_servo);
- wait(2);
- }
- }
- }
-*/
+ }
+ data7=0x10;
+ send_data(0x24,data7);
+
+ }
+ }
+ }
+}
+*/
/*
+void sequence_kaisyu_short(){
+ if(q==0){
+ if(old_triangle!=triangle){
+ old_triangle=triangle;
+ if(triangle==1){
+
+ data_servo=0x07;
+ send_data(0x18,data_servo);
+ wait(1.3);
+ data7=0x00;
+ send_data(0x24,data7);
+ wait(0.3);
+ data7=0x10;
+ send_data(0x24,data7);
+ data_servo=0x08;
+ send_data(0x18,data_servo);
+ wait(1.3);
+ data7=0xff;
+ get_data_rs232=get_data_rs232 & 0x;
+ while(!(get_data_rs232 & 0x20)) {
+ send_data(0x24,data7);
+ get_data_rs232=slave.getc();
+ }
+ data7=0x10;
+ send_data(0x24,data7);
+
+ }
+ }
+ }
+}*/
+
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;
+ if(q==0){
+ if(old_migi1!=migi1){
+ old_migi1=migi1;
+ if(migi1==1&&select==0){
+ 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;
}
- }else if(migi1==0&&hidari1==1){
- data_servo=0x08;
- if(get_data_rs232==0x38){
- data11=0x10;
- }else if(get_data_rs232!=0x38){
+ 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;
}
- }else{
- data11=0x80;
- }
+ }
}
-*/
+
//ファン
-void change_fan(){
- if(left2==1){
+void change_fan()
+{
+ if(q==1){
+ if(left2==1&&(old_triangle==triangle)) {
data8=0xff;
data9=0xff;
data10=0xff;
- }else if(left2==0){
+ } 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);
+ }
}
-
+}
+
+}
-
-
//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);
- }
\ No newline at end of file
+}
+