2019NHK_teamA
NHK2019 manual program(回収機構ストール問題解決)
Diff: main.cpp
- Revision:
- 2:e462c8257384
- Parent:
- 1:99294241f2ba
- Child:
- 3:8c4e42cba9cb
--- a/main.cpp Thu Sep 05 09:40:50 2019 +0000
+++ b/main.cpp Thu Sep 05 10:28:09 2019 +0000
@@ -45,7 +45,7 @@
char data9;//左ファン
char data10;//パトランプ
char data11;//吐き出し機構
-char get_data_rs232=0x80;
+char get_data_rs232=0x00;
int Ry;
int Rx;
int Ly;
@@ -103,10 +103,11 @@
//メイン関数
-int main(){
+int main()
+{
led=1;
initialization();
- while(true){
+ while(true) {
emergency();
get_data();
change_pwm();
@@ -115,9 +116,9 @@
change_rack();
change_rack_2();
sequence_kaisyu();
- //kaisyu();
- sequence_hakidashi();
- //hakidashi();
+ //kaisyu();
+ sequence_hakidashi();
+ //hakidashi();
change_fan();
send_data(0x10,data1);
send_data(0x12,data2);
@@ -131,16 +132,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 +154,7 @@
data9=0x80;
data10=0x80;
data11=0x80;
-
+
send_data(0x10,data1);
send_data(0x12,data2);
send_data(0x14,data3);
@@ -164,13 +166,14 @@
send_data(0x28,data9);
send_data(0x40,data10);
send_data(0x30,data11);
-
-
- }
+
+
+}
-
+
//データ読み込み
-void get_data(){
+void get_data()
+{
Ry=ps3.getRightJoystickYaxis();
Rx=ps3.getRightJoystickXaxis();
Ly=ps3.getLeftJoystickYaxis();
@@ -189,241 +192,245 @@
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;
- }
+ } 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){
+ 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 change_data(){
-
- if(Ry!=0||Rx!=0){
+void change_data()
+{
+
+ if(Ry!=0||Rx!=0) {
square=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){
- 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){
+ } 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;
- 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){
+ } 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(select!=old_select) {
+ old_select=select;
+ if(select==1) {
+ if(i==1) {
+ pwm=0;
+ i=0;
+ } else if(i==0) {
+ pwm=1;
+ 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;
+ //ハンガー機構
+ 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){
old_hidari1=hidari1;
-
+
if(hidari1==1&&select==0){
if(n==1){
data_servo=0x05;
@@ -434,77 +441,73 @@
}
}
}*/
- }
-
+}
+
//ラック
-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){
- data5=0x10;
- data6=0x10;
- }else if(get_data_rs232==0x22){
+void change_rack_2()
+{
+ if(triangle==0&&cross==0&&ue1==0&&shita==0&&Ly>32) {
+ if(get_data_rs232 & 0x01) {
data5=0x10;
- data6=0xff;
- }else if(get_data_rs232==0x24){
+ } else {
data5=0x00;
+ }
+ if(get_data_rs232 & 0x02) {
data6=0x10;
- }else{
- data5=0x00;
+ } else {
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){
+ } else if(triangle==0&&cross==0&&ue1==0&&shita==0&&Ly<-32) {
+ if(get_data_rs232 & 0x04) {
data5=0x10;
- data6=0x00;
- }else if(get_data_rs232==0x30){
+ } else {
data5=0xff;
+ }
+ if(get_data_rs232 & 0x08) {
data6=0x10;
- }else{
- data5=0xff;
+ } else {
data6=0x00;
}
- }else if(triangle==0&&cross==0&&ue1==0&&shita==0&&(-32<Ly<32)){
+ } else if(triangle==0&&cross==0&&ue1==0&&shita==0&&(-32<Ly<32)) {
data5=0x80;
data6=0x80;
}
@@ -512,36 +515,37 @@
//回収機構
-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);
+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 & 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(2);
+ data7=0xff;
+ while(!(get_data_rs232 & 0x20)) {
+ send_data(0x24,data7);
+ get_data_rs232=slave.getc();
+ }
+ data7=0x10;
+ send_data(0x24,data7);
+ data_servo=0x07;
+ send_data(0x18,data_servo);
+
+ }
+ }
}
@@ -555,9 +559,9 @@
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){
@@ -572,34 +576,35 @@
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);*/
+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;
- send_data(0x30,data11);
- }
- }else if(hidari1!=old_hidari1){
- if(hidari1==1){
- data11=0x00;
- send_data(0x30,data11);
- }
- } /* else if(hidari1==1){
+ 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);
@@ -611,17 +616,17 @@
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);
-
- }
- }
-
+
+ if((get_data_rs232 & 0x80)&&(data11==0x00)) {
+ data11=0x10;
+ send_data(0x30,data11);
+ } else if((get_data_rs232 & 0x40)&&(data11==0xff)) {
+ data11=0x10;
+ send_data(0x30,data11);
+
+ }
+}
+
/*
void hakidashi(){
@@ -641,29 +646,31 @@
}
}else{
data11=0x80;
- }
+ }
}
*/
//ファン
-void change_fan(){
- if(left2==1){
+void change_fan()
+{
+ if(left2==1) {
data8=0xff;
data9=0xff;
data10=0xff;
- }else if(left2==0){
- data8=0x00;
- data9=0x00;
- data10=0x00;
- }
+ } 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);
@@ -672,5 +679,5 @@
tushin_check=i2c.write(data);
i2c.stop();
wait(0.003);
- }
-
+}
+