2019NHK_teamA
NHK2019 manual program(回収機構ストール問題解決)
Diff: main.cpp
- Revision:
- 3:8c4e42cba9cb
- Parent:
- 2:e462c8257384
- Child:
- 4:4d9c0999a90a
--- a/main.cpp Thu Sep 05 10:28:09 2019 +0000
+++ b/main.cpp Thu Sep 12 09:03:59 2019 +0000
@@ -80,8 +80,11 @@
int old_left1=0;
int p=2;
int q=0;
-
-
+int old_ue1=0;
+int old_shita=0;
+int old_left2=0;
+int old_triangle=0;
+int old_cross=0;
//関数プロトタイプ宣言
@@ -90,7 +93,7 @@
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();
@@ -99,6 +102,7 @@
//void kaisyu();
void sequence_hakidashi();
//void hakidashi();
+void mode_change();
@@ -106,14 +110,16 @@
int main()
{
led=1;
+ 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();
//kaisyu();
@@ -167,7 +173,19 @@
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);
+ }
+
+
+
}
@@ -230,12 +248,33 @@
}
+//モードチェンジ
+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) {
square=0;
+ migi1=0;
}
if(Ry>30&&i==0) {
@@ -256,10 +295,22 @@
data4=0x00;
}
} else if(Ry<-30&&i==0) {
- data1=0xff;
- data2=0xff;
- data3=0xff;
- data4=0xff;
+ if(right1==1){
+ data1=0xdf;
+ data2=0xff;
+ data3=0xdf;
+ data4=0xff;
+ }else if(left1==1){
+ data1=0xff;
+ data2=0xdf;
+ data3=0xff;
+ data4=0xdf;
+ }else{
+ data1=0xff;
+ data2=0xff;
+ data3=0xff;
+ data4=0xff;
+ }
} else if(Rx>30&&i==0) {
if(right1==1) {
data1=0xff;
@@ -322,10 +373,22 @@
data4=0x3f;
}
} else if(Ry<-30&&i==1) {
- data1=0xbf;
- data2=0xbf;
- data3=0xbf;
- data4=0xbf;
+ 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(Rx>30&&i==1) {
if(right1==1) {
data1=0xbf;
@@ -382,14 +445,18 @@
//pwm変化
void change_pwm()
{
- if(select!=old_select) {
- old_select=select;
- if(select==1) {
+ 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;
}
}
@@ -444,6 +511,7 @@
}
//ラック
+/*
void change_rack()
{
@@ -481,10 +549,12 @@
}
}
+*/
//ラック左右
void change_rack_2()
{
+ /*
if(triangle==0&&cross==0&&ue1==0&&shita==0&&Ly>32) {
if(get_data_rs232 & 0x01) {
data5=0x10;
@@ -511,12 +581,46 @@
data5=0x80;
data6=0x80;
}
+ */
+
+ if(ue1!=old_ue1){
+ old_ue1=ue1;
+ if(ue1==1){
+ data5=0x00;
+ data6=0xff;
+ }
+ }else if(shita!=old_shita){
+ old_shita=shita;
+ if(shita==1){
+ data5=0xff;
+ data6=0x00;
+ }
+ }
+
+ 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) {
@@ -541,9 +645,8 @@
}
data7=0x10;
send_data(0x24,data7);
- data_servo=0x07;
- send_data(0x18,data_servo);
+ }
}
}
}
@@ -583,28 +686,47 @@
void sequence_hakidashi()
{
+ if(q==0){
if(migi1!=old_migi1) {
old_migi1=migi1;
- if(migi1==1) {
- /* data_servo=0x07;
+ if(migi1==1&&select==0) {
+ data_servo=0x07;
send_data(0x18,data_servo);
- wait(1.5);
+ wait(1);
+ data7=0x00;
+ send_data(0x24,data7);
+ wait(0.5);
+ data7=0x10;
+ send_data(0x24,data7);
data11=0xff;
- while(get_data_rs232!=0x36){
+ while(!(get_data_rs232 & 0x40)){
send_data(0x30,data11);
get_data_rs232=slave.getc();
- }
- data11=0x10;
- send_data(0x30,data11);*/
- data11=0xff;
- send_data(0x30,data11);
+ }
+ 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);
+
}
- } else if(hidari1!=old_hidari1) {
+ } /*else if(hidari1!=old_hidari1) {
if(hidari1==1) {
data11=0x00;
send_data(0x30,data11);
}
- } /* else if(hidari1==1){
+ } else if(hidari1==1){
if(get_data_rs232!=0x38){
data11=0x38;
send_data(0x30,data11);
@@ -612,11 +734,11 @@
data11=0x10;
send_data(0x30,data11;
}
- }*/ /*else{
+ } */ /*else{
data11=0x80;
send_data(0x30,data11);
}*/
-
+ /*
if((get_data_rs232 & 0x80)&&(data11==0x00)) {
data11=0x10;
send_data(0x30,data11);
@@ -624,6 +746,7 @@
data11=0x10;
send_data(0x30,data11);
+ }*/
}
}
@@ -654,16 +777,42 @@
//ファン
void change_fan()
{
- if(left2==1) {
+ 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(1);
+ 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);
+ }
}
}
+
+}
@@ -673,11 +822,9 @@
{
tsushin=1;
tushin_check=0;
- i2c.frequency(100000);
i2c.start();
i2c.write(address);
tushin_check=i2c.write(data);
i2c.stop();
- wait(0.003);
}