Diff: main.cpp

Revision:

0:18789c50854e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Dec 25 06:17:55 2019 +0000
@@ -0,0 +1,278 @@
+#include "mbed.h"
+#include "QEI.h"
+#include "PS3.h"
+
+
+//エンコーダー個数
+#define encoder_MAX 4
+
+//PS3ボタン個数
+#define ps3_MAX 3
+
+//ショートブレーキデータ
+#define SBREAK 0x80
+
+//i2c用ピン
+I2C i2c(PB_9,PB_8);
+
+//SBDBT用通信ポート
+PS3 ps3(D8,D2);
+
+//PCとの通信用
+Serial pc(USBTX,USBRX);//D14,D15
+
+//LED
+DigitalOut green(PB_7);     //電源確認
+DigitalOut yellow(PC_2);    //i2c確認
+DigitalOut red(PC_3);       //動作確認
+DigitalOut blue(PC_13);     //12V確認
+
+//12V停止ピン
+DigitalOut emergency(D10);
+
+
+//エンコーダー用ピン
+QEI wheel_RF(PA_8,  PA_6,  NC, 600);    //encoder1
+QEI wheel_RB(PB_14, PB_13, NC, 600);    //encoder2
+QEI wheel_LF(PB_1 , PB_15, NC, 600);    //encoder3
+QEI wheel_LB(PA_12, PA_11, NC, 600);    //encoder4
+
+//パルス、rpm取得用
+Ticker get_rpm;
+
+//MD用アドレスとデータ格納用変数
+char data_RF[1]={};
+char data_RB[1]={};
+char data_LF[1]={};
+char data_LB[1]={};
+
+//初期化MDデータ
+char init_data[1]={0x80};
+
+//PS3ジョイスティックデータ格納用変数
+int ps3_stick[4]={};         //0:右ジョイスティックY軸 1:右ジョイスティックX軸 2:左ジョイスティックY軸 3:左ジョイスティックX軸
+
+//PS3ボタンデータ格納変数
+bool ps3_button[ps3_MAX]={};
+
+//rpm,パルス、合計パルス格納変数
+double rpm[encoder_MAX]={};
+int pulse[encoder_MAX]={};
+int sum_pulse[encoder_MAX]={};
+
+//関数プロトタイプ宣言
+void initialization();
+void stop_12V();
+void get_data();
+void change_data();
+void send_data();
+void change_rpm();
+void print();
+
+int main(){
+    
+    initialization();
+    
+    get_rpm.attach_us(&change_rpm,40000);
+    
+    while(true)
+    {
+        get_data();
+        stop_12V();
+        change_data();
+        send_data();
+        print();
+    }
+}
+
+//初期化
+void initialization(){
+    
+    bool i2c_check[4]={1,1,1,1};    //0:成功 0以外:失敗
+    
+    green=1;
+    blue=1;
+    pc.baud(115200);
+    i2c.frequency(100000);
+    
+    //エンコーダーリセット
+    wheel_RF.reset();
+    wheel_RB.reset();
+    wheel_LF.reset();
+    wheel_LB.reset();
+    
+    i2c_check[0]=i2c.write(0x10,init_data,1);
+    i2c_check[1]=i2c.write(0x12,init_data,1);
+    i2c_check[2]=i2c.write(0x14,init_data,1);
+    i2c_check[3]=i2c.write(0x16,init_data,1);
+    
+    if(i2c_check[0]!=0||i2c_check[1]!=0||i2c_check[2]!=0||i2c_check[3]!=0)
+        yellow=0;
+    else
+        yellow=1;
+        
+    emergency=0;
+    
+}    
+
+//緊急停止
+void stop_12V(){
+    
+    static bool old_data=0;
+    static bool flag=0;
+    
+    if(old_data!=ps3_button[0])
+    {
+        old_data=ps3_button[0];
+        if(ps3_button[0]==1)
+        {
+            if(flag==0)
+            {
+                flag=1;
+                emergency=1;
+                blue=0;
+            }
+            else
+            {
+                flag=0;
+                emergency=0;
+                blue=1;
+            }
+        }
+    }
+     
+}
+
+//rpm,合計パルス取得取得
+void change_rpm(){
+    
+    //パルス取得
+    pulse[0]=wheel_RF.getPulses();
+    pulse[1]=wheel_RB.getPulses();
+    pulse[2]=wheel_LF.getPulses();
+    pulse[3]=wheel_LB.getPulses();
+    
+    //エンコーダーリセット
+    wheel_RF.reset();
+    wheel_RB.reset();
+    wheel_LF.reset();
+    wheel_LB.reset();
+    
+    //合計パルス取得
+    for(int i=0;i<encoder_MAX;i++){
+        sum_pulse[i]=sum_pulse[i]+pulse[i];
+    }
+    
+    //rpm取得
+    for(int j=0;j<encoder_MAX;j++){
+        rpm[j]=60*25*(double)pulse[j]/1200;
+    }
+    
+}
+
+//PS3データ取得
+void get_data(){
+    
+    ps3_stick[0]=ps3.getRightJoystickYaxis();
+    ps3_stick[1]=ps3.getRightJoystickXaxis();
+    ps3_stick[2]=ps3.getLeftJoystickYaxis();
+    ps3_stick[3]=ps3.getLeftJoystickXaxis();
+    ps3_button[0]=ps3.getSELECTState();
+    ps3_button[1]=ps3.getButtonState(R1);
+    ps3_button[2]=ps3.getButtonState(L1);
+    
+    if(ps3_stick[0]==0&&ps3_stick[1]==0&&ps3_stick[2]==0&&ps3_stick[3]==0&&ps3_button[0]==0&&ps3_button[1]==0&&ps3_button[2]==0)
+        red=0;
+    else
+        red=1;
+    
+}
+
+//MDデータ変更
+void change_data(){
+    
+    
+    if(ps3_stick[0]>30){
+        //前進
+        data_RF[0]=0xff; data_RB[0]=0x00; data_LF[0]=0x00; data_LB[0]=0xff;
+    }
+    else if(ps3_stick[0]<-30){
+        //後進
+        data_RF[0]=0x00; data_RB[0]=0xff; data_LF[0]=0xff; data_LB[0]=0x00;
+    }
+    else if(ps3_stick[1]>30){
+        //右
+        data_RF[0]=0x00; data_RB[0]=0x00; data_LF[0]=0x00; data_LB[0]=0x00;
+    }
+    else if(ps3_stick[1]<-30)
+    {
+        //左
+        data_RF[0]=0xff; data_RB[0]=0xff; data_LF[0]=0xff; data_LB[0]=0xff;
+    }
+    else if((ps3_stick[2]>30)&&(ps3_stick[3]>30))
+    {
+        //右斜め上
+        data_RF[0]=0x80; data_RB[0]=0x00; data_LF[0]=0x00; data_LB[0]=0x80;
+        
+    }
+    else if((ps3_stick[2]<-30)&&(ps3_stick[3]>30))
+    {
+        //右斜め下
+        data_RF[0]=0x00; data_RB[0]=0x80; data_LF[0]=0x80; data_LB[0]=0x00;
+        
+    }
+    else if((ps3_stick[2]>30)&&(ps3_stick[3]<-30))
+    {
+        //左斜め上
+        data_RF[0]=0xff; data_RB[0]=0x80; data_LF[0]=0x80; data_LB[0]=0xff;
+        
+    }
+    else if((ps3_stick[2]<-30)&&(ps3_stick[3]<-30))
+    {
+        //左斜め下
+        data_RF[0]=0x80; data_RB[0]=0xff; data_LF[0]=0xff; data_LB[0]=0x80;
+        
+    }
+    else if(ps3_button[1]==1)
+    {
+        //右旋回
+        data_RF[0]=0x00; data_RB[0]=0x00; data_LF[0]=0xff; data_LB[0]=0xff;
+    }
+    else if(ps3_button[2]==1)
+    {
+        //左旋回
+        data_RF[0]=0xff; data_RB[0]=0xff; data_LF[0]=0x00; data_LB[0]=0x00;
+    }
+    else
+    {
+        //停止
+        data_RF[0]=SBREAK; data_RB[0]=SBREAK; data_LF[0]=SBREAK; data_LB[0]=SBREAK;
+    }
+
+}
+
+void send_data(){
+    
+    bool i2c_check[4]={1,1,1,1};           //0:成功 0以外:失敗
+    
+    i2c_check[0]=i2c.write(0x10,data_RF,1);
+    i2c_check[1]=i2c.write(0x12,data_RB,1);
+    i2c_check[2]=i2c.write(0x14,data_LF,1);
+    i2c_check[3]=i2c.write(0x16,data_LB,1);
+    
+    if(i2c_check[0]!=0||i2c_check[1]!=0||i2c_check[2]!=0||i2c_check[3]!=0)
+        yellow=0;
+    else
+        yellow=1;
+    
+}
+
+//表示用関数
+void print(){
+    
+    //ps3データ
+    //pc.printf("RY:%d Rx:%d select:%d R1:%d L1:%d\n",ps3_stick[0],ps3_stick[1],ps3_button[0],ps3_button[1],ps3_button[2]);
+    //合計パルス,rpm
+    pc.printf("RF_pulse:%d RB_pulse:%d LF_pulse:%d LB_pulse:%d RF_rpm:%lf RB_rpm:%lf LF_rpm:%lf LB_rpm:%lf\n",sum_pulse[0],sum_pulse[1],sum_pulse[2],sum_pulse[3],rpm[0],rpm[1],rpm[2],rpm[3]);
+    
+}
\ No newline at end of file