Revision 1:0867d6d1421a, committed 2022-03-12

Comitter:

darkumatar

Date:

Sat Mar 12 01:21:56 2022 +0000

Parent:

0:e22d70b99d51

Commit message:

a

Changed in this revision

--- a/Servo.lib	Sat Mar 05 05:00:55 2022 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://os.mbed.com/users/simon/code/Servo/#36b69a7ced07

--- a/air.lib	Sat Mar 05 05:00:55 2022 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://os.mbed.com/teams/Akashi_2021_Bteam/code/air/#409232b72b2b

--- a/arrc_mbed.lib	Sat Mar 05 05:00:55 2022 +0000
+++ b/arrc_mbed.lib	Sat Mar 12 01:21:56 2022 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/TanakaRobo/code/arrc_mbed/#7ea663f79c81
+https://os.mbed.com/teams/go-roboB/code/arrc_mbed/#7ea663f79c81

--- a/main.cpp	Sat Mar 05 05:00:55 2022 +0000
+++ b/main.cpp	Sat Mar 12 01:21:56 2022 +0000
@@ -3,15 +3,13 @@
 #include "rotary_inc.hpp"
 #include"cmath"
 #include"gy521.hpp"
-
-#include "air.hpp"
-#include "Servo.h"
+#include "neopixel.h"
 
 
-#define goal_x 14000.0
-#define goal_y 15000.0
-#define period_r 3000.0
-
+#define goal_x -15000.0
+#define goal_y 14000.0
+#define period_r 6000.0
+NeoPixelOut npx(PB_4,7);
 I2C i2d(PB_3,PB_10);
 GY521 gyro(i2d);
 ScrpSlave slave(PC_12,PD_2 ,PH_1 ,SERIAL_TX,SERIAL_RX,0x0807f800);//srcslave設定
@@ -21,17 +19,26 @@
 int angler=90;
 int speedr=1024;
 bool auto_swich = false;
+bool limit_switch = true;
 int step=0;
+int auto_mode=0;
 
 
+int check_tepu=0;
+
+bool limi=true;
+
+bool yabe=false;
+
+DigitalIn limitB(PB_12,PullUp);
 
 
 
 
 bool add(int id,int ppp){//速度を変えるやつ
-    if(auto_swich==false){
-        target[id]=20*ppp;
-    }    
+    if(auto_swich==false&&limi==true){
+        target[id]=30*ppp;
+    }
     return true;
 }
 
@@ -86,16 +93,30 @@
     return true;
 }
 
+bool limit2(int id,int ppp){
+    if(ppp==1){
+        limit_switch=false;
+    }
+    return true;
+}
+
+bool  limit(int a,int &b){
+    return limit2(0,a);
+}
 bool  abc1(int a,int &b){
+    check_tepu+=1;
     return add(0,a);
 }
 bool  abc2(int a,int &b){
+    check_tepu+=1;
     return add(1,a);
 }
 bool  abc3(int a,int &b){
+    check_tepu+=1;
     return add(2,a);
 }
 bool  abc4(int a,int &b){
+    check_tepu+=1;
     return add(3,a);
 }
 bool  abc5(int a,int &b){
@@ -116,19 +137,26 @@
 bool  abc10(int a,int &b){
     return auto_off(0,a);
 }
+
+bool  abc22(int a,int &b){
+    yabe=true;
+    return true;
+}
+
 int main() {
-    slave.port2.printf("%d\n",1);
+   // slave.addCMD(1,limit);
     slave.addCMD(2,abc1);
     slave.addCMD(3,abc2);
     slave.addCMD(4,abc3);
     slave.addCMD(5,abc4);
+    slave.addCMD(6,abc22);
    // slave.addCMD(6,abc5);
    // slave.addCMD(7,abc6);
    // slave.addCMD(8,abc7);
    // slave.addCMD(9,abc8);
     slave.addCMD(12,abc9);
     slave.addCMD(13,abc10);
-    
+    slave.addCMD(1,limit);
 
     int i;    
     double x_period=0;
@@ -152,6 +180,8 @@
     speed_pwm[i]=0;
    }
     Timer name;
+    Timer limilimi;
+    limilimi.start();
     name.start();
     gyro.start();
     double angle=0;
@@ -160,7 +190,8 @@
     int test_x=0;
     int test_y=0;
     int test_a=0;
-    while(1) {
+    bool limilimi_swich=false;
+    while(limit_switch==true) {
     angle=45.0+gyro.yaw;
     
     if(auto_swich==true){
@@ -195,6 +226,34 @@
         }
     }
     
+    
+    
+        if(limitB){
+            limi=true;
+            limilimi_swich=false;
+        }else{
+            int arc=0;
+            for(i=0;i<4;i++){
+                arc+=target[i]*sin((45.0+90.0*i)/90.0*M_PI);
+            }
+            if(arc>0){
+                //target[i]=0;
+            }
+            if(limilimi_swich==false){
+                slave.send1(255,20,2);
+                limilimi_swich=true;
+                limilimi.reset();
+            }
+        }
+        if(limilimi.read_ms()<500&&!limitB){
+            limi=false;
+            for(i=0;i<4;i++){
+                target[i] =0;   
+            }
+        }else {
+            limi=true;
+        }
+    
         for(i=0;i<4;i++){
             after[i]=v[i].get();//ロリコンから値読み取り
             integral[i]+= (((((target[i]/10.0-(after[i]-before[i]))+ before_parus[i])*0.1)/2.0)/102.40);//積分の所
@@ -217,13 +276,62 @@
         test_y=y_period;
         test_a=angle;
         slave.port2.printf("%d\n",1);
-        slave.send1(255,30,test_x);
+        //slave.send1(255,30,test_x);
         while(name.read_ms()<30){}
-        slave.send1(255,31,test_y);
+        //slave.send1(255,31,test_y);
         while(name.read_ms()<60){}
-        slave.send1(255,32,test_a);
+        //slave.send1(255,32,test_a);
         while(name.read_ms()<100){}
+        
+        for(i = 0; i < 4; i++){
+        npx.global_scale = 0.05f;
+        npx.normalize = false;
+            if(check_tepu>0){
+            npx.setPixelColor(i,0xFF0000);
+            }else {
+            npx.setPixelColor(i,0x0000FF);
+            }
+        }
+        for(i = 4; i < 7; i++){
+        npx.global_scale = 0.05f;
+        npx.normalize = false;
+        if(auto_swich){
+            npx.setPixelColor(i,0xFFFFFF);
+        }else {
+            npx.setPixelColor(i,0x00FF00);
+        }
+        }
+         npx.show();
+        if(check_tepu==0){
+            auto_mode+=1;
+        }else {
+            auto_mode=0;    
+        }
+        
+        if(auto_mode>5){
+            limit_switch=false;
+        }
+        
+        check_tepu=0;
+        
         gyro.update();
         name.reset();
     }
+    
+  
+    while(1){
+        for(i=0;i<4;i++){
+        target[i]=0;
+        motor_p[i]=0;
+        motor_m[i]=0;
+        
+        npx.global_scale = 0.05f;
+        npx.normalize = false;
+        npx.setPixelColor(i,0x00FFFF);
+        
+        
+        }
+    npx.show();
+    }
+    
 }