Revision 5:e189d55ef292, committed 2022-01-28

Comitter:

kazumayamanaka

Date:

Fri Jan 28 15:12:04 2022 +0000

Parent:

1:ea880e226e5a

Commit message:

program of wheel;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AutoMatic.cpp	Fri Jan 28 15:12:04 2022 +0000
@@ -0,0 +1,28 @@
+#include "AutoMatic.hpp"
+
+double AutoMatic::CalcDiffX(){ return  TargetPosX - PosX; }
+
+void AutoMatic::pass_TargetPosX(double X){
+    TargetPosX = X;
+}
+
+void AutoMatic::pass_PosX(double X){
+    PosX = X;
+}
+
+void AutoMatic::CalcPowerX(){
+    if(CalcDiffX()>0){
+        Xpower = 100;
+    }
+    else{
+        Xpower = 0;
+    }
+}
+
+double AutoMatic::obt_Xpower(){
+    CalcPowerX();
+    return Xpower; 
+}
+        
+    
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AutoMatic.hpp	Fri Jan 28 15:12:04 2022 +0000
@@ -0,0 +1,23 @@
+#ifndef AUTO_MATIC_H
+#define AUTO_MATIC_H
+
+#include "mbed.h"
+
+class AutoMatic{
+    public:
+        void pass_TargetPosX(double X);
+        void pass_PosX(double X);
+        double obt_Xpower();
+        
+    private:
+        double TargetPosX,TargetPosY;
+        double PosX,PosY;
+        double Xpower,Ypower;
+        double diffX,diffY;
+        
+        double CalcDiffX();
+        void CalcPowerX();
+    
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Odmetry.cpp	Fri Jan 28 15:12:04 2022 +0000
@@ -0,0 +1,60 @@
+#include "Odmetry.hpp"
+
+Odmetry::Odmetry(){
+    dt = 0.05;
+    Radius = 395;
+    Wradius = 25;
+    XPos = 0;
+    YPos = 0;
+}
+
+void Odmetry::pass_pulse(double PULSE1,double PULSE2,double PULSE3,double PULSE4){
+    pulse_a[0] = PULSE1;
+    pulse_a[1] = PULSE2;
+    pulse_a[2] = PULSE3;
+    pulse_a[3] = PULSE4;
+    
+    if(PULSE1 == PULSE2 == PULSE3 == PULSE4){
+        return;
+    }
+    else{
+        CalcSpd();
+        Angular();
+        transform();
+        CalcPosition();
+    }
+}
+
+void Odmetry::pass_angle(double THETA){
+    Theta = THETA - PI / 4;
+}
+    
+
+void Odmetry::CalcSpd(){
+    for(int i = 0;i < 4;i ++){
+        diff[i] = pulse_a[i] - pulse_b[i];
+        spd[i] = Co * diff[i] / dt;
+        pulse_b[i] = pulse_a[i];
+    }
+}
+
+void Odmetry::Angular(){
+    for(int i = 0;i < 4;i ++){
+        Omega[i] = spd[i] / Radius;
+    }
+}
+
+void Odmetry::transform(){
+    Xpower = Wradius * (Omega[1] - Omega[3]);
+    Ypower = Wradius * (Omega[0] - Omega[2]);
+}
+
+void Odmetry::CalcPosition(){
+    Xspd = cos(Theta) * Xpower - sin(Theta) * Ypower;
+    Yspd = sin(Theta) * Xpower + cos(Theta) * Ypower;
+    
+    XPos += Xspd * dt;
+    YPos += Yspd * dt;
+}
+
+void Odmetry::print_Pos(){ printf("XPos:%lf YPos:%lf theta:%lf\n",XPos,YPos,Theta); }

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Odmetry.hpp	Fri Jan 28 15:12:04 2022 +0000
@@ -0,0 +1,33 @@
+#ifndef ODMETRY_H
+#define ODMETRY_H
+
+#include "mbed.h"
+#define Co (108 * 3.1415926535897) / 512
+#define PI 3.1415926535897
+
+class Odmetry{
+    public:
+        Odmetry();
+        void pass_pulse(double PULSE1,double PULSE2,double PULSE3,double PULSE4);
+        void pass_angle(double THETA);
+        void print_Pos();
+        
+    private:
+        void Angular();
+        void CalcPosition();
+        void CalcSpd();
+        void transform();
+        
+        double Radius;
+        double Wradius;
+        double Omega[4];
+        double spd[4];
+        double pulse_a[4],pulse_b[4],diff[4];
+        double Xspd,Yspd;
+        double Xpower,Ypower;
+        double XPos,YPos,Theta;
+        double dt;
+};
+
+#endif
+        
\ No newline at end of file

--- a/PIDco.cpp	Thu Jan 20 07:27:53 2022 +0000
+++ b/PIDco.cpp	Fri Jan 28 15:12:04 2022 +0000
@@ -1,7 +1,7 @@
 #include "PIDco.hpp"
 
 PIDco::PIDco(){
-    spd =  0.0;
+    spd = 0.0;
     Pwm = 0.0;
     dt = 0.05;
     Integral = 0.0;
@@ -31,8 +31,8 @@
     PwmOut v1p(PIN_A);
     PwmOut v1m(PIN_B);
     
-    v1p.period_us(2048);
-    v1m.period_us(2048);
+    v1p.period_us(1024);
+    v1m.period_us(1024);
     
     cal_spd();
     cal_Error();

--- a/main.cpp	Thu Jan 20 07:27:53 2022 +0000
+++ b/main.cpp	Fri Jan 28 15:12:04 2022 +0000
@@ -3,6 +3,7 @@
 #include "PIDco.hpp"
 #include "TARGETco.hpp"
 #include "DUALSHOCKco.hpp"
+#include "Odmetry.hpp"
 #include "sbdbt.hpp"
 #include "BNO055.h"
 
@@ -17,6 +18,11 @@
 RotaryInc data_3(PC_0,PC_1,0);
 RotaryInc data_4(PC_2,PC_3,0);
 
+RotaryInc MW_1(PA_13,PC_4,0);
+RotaryInc MW_2(PC_10,PC_11,0);
+RotaryInc MW_3(PA_7,PA_6,0);
+RotaryInc MW_4(PA_8,PA_9,0);
+
 PIDco pid_1;
 PIDco pid_2;
 PIDco pid_3;
@@ -25,17 +31,17 @@
 TARGETco TG;
 BNO055 bno(SDA,SCL);
 DUALSHOCKco DS;
-
+Odmetry odmetry;
 sbdbt sb(PA_0,PA_1);
 
 Timer Time;
 double timer;
 double theta;
-double X,Y;
 
 int main(){
     Time.start();
     
+    
     while(1){
         timer = Time.read_us();
         bno.setmode(OPERATION_MODE_IMUPLUS);
@@ -47,6 +53,8 @@
             theta = -(2 * PI - theta);
         }
         
+        theta = theta - PI / 4;
+        
         DS.pass_val(sb.rsx(),sb.rsy(),sb.r2An(),sb.l2An());
         
         if(DS.cal_input() == true){
@@ -66,7 +74,10 @@
         pid_3.wheel_ctl(PB_5,PB_4);
         pid_4.wheel_ctl(PB_7,PB_6);
         
-        pc.printf("%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf\n",TG.obt_target1(),TG.obt_target2(),TG.obt_target3(),TG.obt_target4(),pid_1.obt_spd(),pid_2.obt_spd(),pid_3.obt_spd(),pid_4.obt_spd());
+        odmetry.pass_pulse(MW_1.get(),MW_2.get(),MW_3.get(),MW_4.get());
+        odmetry.pass_angle(theta);
+        odmetry.print_Pos();
+        //pc.printf("%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf\n",TG.obt_target1(),TG.obt_target2(),TG.obt_target3(),TG.obt_target4(),pid_1.obt_spd(),pid_2.obt_spd(),pid_3.obt_spd(),pid_4.obt_spd());
         while(Time.read_us() - timer <= 50 * 1000);
         }
 }