2019NHK_teamA
NHK2019 manual program(回収機構ストール問題解決)
Diff: main.cpp
- Revision:
- 0:c7e17c2fd542
- Child:
- 1:99294241f2ba
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Jun 25 08:53:27 2019 +0000
@@ -0,0 +1,354 @@
+/////////////////////////////////////
+/*NHK2019*/
+/*Aチーム手動機プログラム*/
+/*担当者: 品川敦哉*/
+/*
+アドレス
+0x10:右前
+0x12:左前
+0x14:右後ろ
+0x16:左後ろ
+0x18:サーボ制御回路
+0x20:右ラック
+0x22:左ラック
+0x24:回収機構
+0x26:右ファン
+0x28:左ファン
+*/
+/////////////////////////////////////
+
+//宣言
+#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 get_data_rs232=0x80;
+int Ry;
+int Rx;
+int right1;
+int left1;
+int left2;
+int select;
+int start;
+int circle;
+int cross;
+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_cross=0;
+int l=1;
+
+
+//関数プロトタイプ宣言
+void get_data();
+void change_data();
+void change_pwm();
+void change_servo();
+void change_rack();
+void change_kaisyu();
+void change_fan();
+void send_data(char address,char data);
+void emergency();
+void get_limit_data();
+
+//メイン関数
+int main(){
+ led=1;
+ while(true){
+ emergency();
+ get_data();
+ change_pwm();
+ change_servo();
+ change_data();
+ change_rack();
+ change_kaisyu();
+ change_fan();
+ get_limit_data();
+ 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(0x24,data7);
+ send_data(0x26,data8);
+ send_data(0x28,data9);
+ }
+ }
+
+//データ読み込み
+void get_data(){
+ Ry=ps3.getRightJoystickYaxis();
+ Rx=ps3.getRightJoystickXaxis();
+ circle=ps3.getButtonState(maru);
+ cross=ps3.getButtonState(batu);
+ left1=ps3.getButtonState(L1);
+ left2=ps3.getButtonState(L2);
+ right1=ps3.getButtonState(R1);
+ select=ps3.getSELECTState();
+ start=ps3.getSTARTState();
+ ue1=ps3.getButtonState(ue);
+ shita=ps3.getButtonState(sita);
+ hidari1=ps3.getButtonState(hidari);
+ migi1=ps3.getButtonState(migi);
+ pc.printf("%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n",Ry,Rx,left1,left2,right1,select,start,ue1,shita,migi1,hidari1,tushin_check,i);
+ if(Ry==0&&Rx==0&&left1==0&&right1==0&&left2==0&&select==0&&start==0&&shita==0&&ue1==0&&migi1==0&&hidari1==0){
+ data_check=0;
+ }else{
+ data_check=1;
+ }
+
+ }
+
+//緊急停止
+void emergency(){
+ if(start!=old_start){
+ old_start=start;
+ if(start==1){
+ if(j==1){
+ //緊急停止
+ data_servo=0x00;
+ send_data(0x18,data_servo);
+ led=0;
+ stop=1;
+ j=0;
+ }else if(j==0){
+ //緊急停止解除
+ led=1;
+ stop=0;
+ j=1;
+ }
+ }
+ }
+}
+
+
+//データ変化(メカナム)
+void change_data(){
+ if(Ry>30&&i==0){
+ data1=0xff;
+ data2=0x00;
+ data3=0xff;
+ data4=0x00;
+ }else if(Ry<-30&&i==0){
+ data1=0x00;
+ data2=0xff;
+ data3=0x00;
+ data4=0xff;
+ }else if(Rx>30&&i==0){
+ data1=0xff;
+ data2=0xff;
+ data3=0x00;
+ data4=0x00;
+ }else if(Rx<-30&&i==0){
+ data1=0x00;
+ data2=0x00;
+ data3=0xff;
+ data4=0xff;
+ }else if(right1==1&&i==0){
+ data1=0x00;
+ data2=0x00;
+ data3=0x00;
+ data4=0x00;
+ }else if(left1==1&&i==0){
+ data1=0xff;
+ data2=0xff;
+ data3=0xff;
+ data4=0xff;
+ }else if(Ry>30&&i==1){
+ data1=0xbf;
+ data2=0x3f;
+ data3=0xbf;
+ data4=0x3f;
+ }else if(Ry<-30&&i==1){
+ data1=0x3f;
+ data2=0xbf;
+ data3=0x3f;
+ data4=0xbf;
+ }else if(Rx>30&&i==1){
+ data1=0xbf;
+ data2=0xbf;
+ data3=0x3f;
+ data4=0x3f;
+ }else if(Rx<-30&&i==1){
+ data1=0x3f;
+ data2=0x3f;
+ data3=0xbf;
+ data4=0xbf;
+ }else if(right1==1&&i==1){
+ data1=0x3f;
+ data2=0x3f;
+ data3=0x3f;
+ data4=0x3f;
+ }else if(left1==1&&i==1){
+ data1=0xbf;
+ data2=0xbf;
+ data3=0xbf;
+ 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(j!=0){
+ if(circle!=old_circle){
+ if(circle==1){
+ if(k==1){
+ data_servo=0x01;
+ k=0;
+ }else if(k==0){
+ data_servo=0x02;
+ k=1;
+ }
+ }
+ }else if(cross!=old_cross){
+ if(cross==1){
+ if(l==1){
+ data_servo=0x03;
+ l=0;
+ }else if(l==0){
+ data_servo=0x04;
+ l=1;
+ }
+ }
+ }
+ }
+ }
+
+//ラック
+void change_rack(){
+ if(ue1==1&&shita==0){
+ data5=0x00;
+ data6=0x00;
+ }else if(ue1==0&&shita==1){
+ data5=0xff;
+ data6=0xff;
+ }else{
+ data5=0x80;
+ data6=0x80;
+ }
+ }
+
+//回収機構
+void change_kaisyu(){
+ if(migi1==1&&hidari1==0){
+ data7=0xff;
+ }else if(migi1==0&&hidari1==1){
+ data7=0x00;
+ }else{
+ data7=0x80;
+ }
+ }
+
+//ファン
+void change_fan(){
+ if(left2==1){
+ data8=0xff;
+ data9=0xff;
+ }else if(left2==0){
+ data8=0x00;
+ data9=0x00;
+ }
+ }
+
+//リミットスイッチデータ取得
+void get_limit_data(){
+ get_data_rs232=slave.getc();
+ if(get_data_rs232==0x20){
+ if(ue1==1){
+ data5=0x80;
+ data6=0x80;
+ }
+ }else if(get_data_rs232==0x22){
+ if(ue1==1){
+ data5=0x80;
+ }
+ }else if(get_data_rs232==0x24){
+ if(ue1==1){
+ data6=0x80;
+ }
+ }else if(get_data_rs232==0x26){
+ if(shita==1){
+ data5=0x80;
+ data6=0x80;
+ }
+ }else if(get_data_rs232==0x28){
+ if(shita==1){
+ data5=0x80;
+ }
+ }else if(get_data_rs232==0x30){
+ if(shita==1){
+ data6=0x80;
+ }
+ }else if(get_data_rs232==0x32){
+ if(migi1==1){
+ data7=0x80;
+ }
+ }else if(get_data_rs232==0x34){
+ if(hidari1==1){
+ data7=0x80;
+ }
+ }
+ }
+
+//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