2019NHK_teamA
A team manual robot program 10/3 もとどうり
Diff: main.cpp
- Revision:
- 0:f99e0ea93e31
- Child:
- 2:283e5bb8a712
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Aug 28 09:03:12 2019 +0000
@@ -0,0 +1,534 @@
+/////////////////////////////////////
+/*NHK2019*/
+//Aチーム手動機プログラム
+/*
+アドレス
+0x10:右前
+0x12:左前
+0x14:右後ろ
+0x16:左後ろ
+0x18:サーボ制御回路
+0x20:右ラック
+0x22:左ラック
+0x24:回収機構
+0x26:右ファン
+0x28:左ファン
+0x30:吐き出し機構
+*/
+/////////////////////////////////////
+
+//宣言
+#include "mbed.h"
+#include "PS3.h"
+I2C i2c(D14,D15);
+Serial pc(USBTX,USBRX);
+Serial slave(PC_6,PC_7);
+PS3 ps3(D8,D2);
+DigitalOut led(D5);//電源確認
+DigitalOut tsushin(D6);//通信確認
+DigitalOut data_check(D7);
+DigitalOut pwm(D4);
+DigitalOut stop(D10);
+DigitalOut led1(LED1);
+
+//変数
+char data1;//右上
+char data2;//左上
+char data3;//右下
+char data4;//左下
+char data_servo;
+char data5;//ラック直動右
+char data6;//ラック直動左
+char data7;//回収機構
+char data8;//右ファン
+char data9;//左ファン
+char data10;//パトランプ
+char data11;//吐き出し機構
+char get_data_rs232=0x80;
+int Ry;
+int Rx;
+int Ly;
+int right1;
+int right2;
+int left1;
+int left2;
+int select;
+int start;
+int circle;
+int cross;
+int triangle;
+int square;
+int ue1;
+int shita;
+int migi1;
+int hidari1;
+int tushin_check;
+int old_select=0;
+int i=1;
+int old_start=0;
+int j=1;
+int old_circle=0;
+int k=1;
+int old_square=0;
+int old_migi1=0;
+int old_hidari1=0;
+int n=1;
+int old_right2=0;
+int o=1;
+int old_right1=0;
+int old_left1=0;
+
+
+
+
+
+//関数プロトタイプ宣言
+void get_data();
+void change_data();
+void change_pwm();
+void change_servo();
+void change_rack();
+void change_fan();
+void send_data(char address,char data);
+void emergency();
+void change_rack_2();
+void sequence_kaisyu();
+//void sequence_hakidashi();
+//void hakidashi();
+
+//メイン関数
+int main(){
+ led=1;
+ data7=0x80;
+ while(true){
+ emergency();
+ get_data();
+ change_pwm();
+ change_servo();
+ change_data();
+ change_rack();
+ change_rack_2();
+ sequence_kaisyu();
+ //sequence_hakidashi();
+ //hakidashi();
+ change_fan();
+ send_data(0x10,data1);
+ send_data(0x12,data2);
+ send_data(0x14,data3);
+ send_data(0x16,data4);
+ send_data(0x18,data_servo);
+ send_data(0x20,data5);
+ send_data(0x22,data6);
+ send_data(0x26,data8);
+ send_data(0x28,data9);
+ send_data(0x40,data10);
+
+ }
+ }
+
+//データ読み込み
+void get_data(){
+ Ry=ps3.getRightJoystickYaxis();
+ Rx=ps3.getRightJoystickXaxis();
+ Ly=ps3.getLeftJoystickYaxis();
+ circle=ps3.getButtonState(maru);
+ cross=ps3.getButtonState(batu);
+ triangle=ps3.getButtonState(sankaku);
+ square=ps3.getButtonState(sikaku);
+ left1=ps3.getButtonState(L1);
+ left2=ps3.getButtonState(L2);
+ right1=ps3.getButtonState(R1);
+ right2=ps3.getButtonState(R2);
+ select=ps3.getSELECTState();
+ start=ps3.getSTARTState();
+ ue1=ps3.getButtonState(ue);
+ shita=ps3.getButtonState(sita);
+ 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){
+ 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){
+ data_check=0;
+ }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 change_data(){
+ 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){
+ 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){
+ data1=0xff;
+ data2=0x00;
+ data3=0xff;
+ data4=0x00;
+ }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){
+ 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){
+ data1=0xbf;
+ data2=0x3f;
+ data3=0xbf;
+ data4=0x3f;
+ }else if(left1==1&&i==1){
+ data1=0x3f;
+ data2=0xbf;
+ data3=0x3f;
+ data4=0xbf;
+ }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;
+ }
+ }
+ }
+}
+
+//サーボモーター
+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){
+ old_hidari1=hidari1;
+
+ if(hidari1==1&&select==0){
+ if(n==1){
+ data_servo=0x05;
+ n=0;
+ }else if(n==0){
+ data_servo=0x06;
+ n=1;
+ }
+ }
+ }*/
+ }
+
+//ラック
+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;
+ }
+ //左
+ }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;
+
+ }
+
+}
+
+//ラック左右
+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){
+ data5=0x10;
+ data6=0xff;
+ }else if(get_data_rs232==0x24){
+ data5=0x00;
+ data6=0x10;
+ }else{
+ 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{
+ data5=0xff;
+ data6=0x00;
+ }
+ }else if(triangle==0&&cross==0&&ue1==0&&shita==0&&(-32<Ly<32)){
+ data5=0x80;
+ data6=0x80;
+ }
+}
+
+//回収機構
+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_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();
+ }
+ data11=0x10;
+ send_data(0x30,data11);
+ wait(0.5);
+ data11=0x00;
+ while(get_data_rs232!=0x38){
+ send_data(0x30,data11);
+ get_data_rs232=slave.getc();
+ }
+ data11=0x10;
+ send_data(0x30,data11);
+ data_servo=0x08;
+ send_data(0x18,data_servo);
+ wait(2);
+ }
+ }
+ }
+*/
+
+/*
+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){
+ 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