Revision 0:9a2904ed0946, committed 2019-09-11

Comitter:

maxnagazumi

Date:

Wed Sep 11 07:35:42 2019 +0000

Commit message:

Auto-machine;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CruizCore_R1370P.lib	Wed Sep 11 07:35:42 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/ROBOSTEP_LIBRARY/code/CruizCore_R1370P/#b034f6d0b378

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Encoder.lib	Wed Sep 11 07:35:42 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/ROBOSTEP_LIBRARY/code/Encoder/#f261038cb88f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Moter-control/moter.h	Wed Sep 11 07:35:42 2019 +0000
@@ -0,0 +1,21 @@
+#include "mbed.h"
+#include "math.h"
+
+double Rm(double vx,double vy)
+{
+    double Rm;
+    Rm = vx * cos(60.0)+ vy * cos(30.0);
+    return Rm;
+}
+double Lm(double vx,double vy)
+{
+    double Lm;
+    Lm = vx * cos(60.0) - vy * cos(30.0) ;
+    return Lm;
+}
+double Tm(double vx,double vy)
+{
+    double Tm;
+    Tm = -vx;
+    return Tm;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID_contorl/PID.h	Wed Sep 11 07:35:42 2019 +0000
@@ -0,0 +1,39 @@
+#include "mbed.h"
+#include "EC.h"
+
+#define RESOLUTION 500
+Ec1multi EC(PA_9,PA_8,RESOLUTION);  //x//1逓倍用class
+Ec1multi EC2(PB_3,PB_5,RESOLUTION); //y
+void calcOmega();
+
+double PID_x(double aim_x){
+    double e_x,Kp,Kd,old_count,Ki,integ_x,P_x,I_x,D_x,PID_x,count;
+    count=EC.getCount();
+    e_x = aim_x - count;
+    Kp = 1/12000;                  //set value
+    P_x = Kp * e_x;
+    Kd = 1/1000;                  //set value
+    I_x = Kd * (count - old_count);
+    old_count = count;
+    Ki = 1/10000;                  //set value
+    integ_x = integ_x + e_x;
+    D_x = Ki * integ_x;
+    PID_x = P_x + I_x + D_x;
+    return PID_x;
+}
+    
+ double PID_y(double aim_y){
+    double e_y,Kp,Kd,old_count2,Ki,integ_y,P_y,I_y,D_y,PID_y,count2;
+    count2=EC2.getCount();
+    e_y = aim_y - count2;
+    Kp = 12000;                  //set value
+    P_y = Kp * e_y;
+    Kd = 1/1000;                  //set value
+    I_y = Kd * (count2 - old_count2);
+    old_count2 = count2;
+    Ki = 1/20000;                  //set value
+    integ_y = integ_y + e_y;
+    D_y = Ki * integ_y;
+    PID_y = P_y + I_y + D_y;
+    return PID_y;
+}   

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Sep 11 07:35:42 2019 +0000
@@ -0,0 +1,102 @@
+#include "mbed.h"
+#include "math.h"
+#include "R1370P.h" 
+#include "EC.h"
+#include "PID.h"
+#include "moter.h"
+
+#define RESOLUTION 500
+PwmOut TF(PB_8);//Top motor Forward
+PwmOut TB(PC_9);//Top motor Backward
+PwmOut RF(PC_8);//Right motor is okay
+PwmOut RB(PC_6);
+PwmOut LF(PB_14);//Left motor ONLY F is okay
+PwmOut LB(PB_15);
+
+PwmOut servo(PA_15);
+
+DigitalOut led1(LED1);
+DigitalIn button(USER_BUTTON);
+
+
+Serial pc(USBTX, USBRX); // tx, rx
+R1370P gyro(PA_11,PA_12);   // tx, rx
+Ticker ticker;
+//Serial pc(USBTX,USBRX);
+//Ec1multi EC(PA_8,PA_9,RESOLUTION);  //1逓倍用class
+//Ec1multi EC2(PB_3,PB_5,RESOLUTION);
+void calcOmega();
+
+
+int main()
+{
+    TF.period_ms(1);
+    TB.period_ms(1);
+    RF.period_ms(1);
+    RB.period_ms(1);
+    LF.period_ms(1);
+    LB.period_ms(1);
+
+    servo.period_ms(20);
+    
+//moter_control
+    double aim_x,aim_y,Vx,Vy,Rm,Lm,Tm;
+    aim_x = ;
+    aim_y = ;
+
+    int count=0,count2=0;
+    double omega,omega2;
+    ticker.attach(&calcOmega,0.5);
+
+    pc.printf("start\n");
+    double angle,rate,x_acc,y_acc,z_acc;  //変数宣言
+    gyro.initialize();    //main関数の最初に一度だけ実行
+    gyro.acc_offset();    //やってもやらなくてもいい
+
+    while(1) {
+        count=EC.getCount();
+        omega=EC.getOmega();
+        count2=EC2.getCount();
+        omega2=EC2.getOmega();
+        pc.printf("x=%d,   ",count);
+        //pc.printf("omega=%f      ",omega);
+        pc.printf("y=%d,   ",count2);
+        //pc.printf("omega=%f      ",omega2);
+        angle=gyro.getAngle();    //角度の値を受け取る
+        rate=gyro.getRate();      //角速度の値を受け取る
+        x_acc=gyro.getX_acc();    //X軸の加速度の値を受け取る
+        y_acc=gyro.getY_acc();    //Y軸の加速度の値を受け取る
+        z_acc=gyro.getZ_acc();    //Z軸の加速度の値を受け取る
+        pc.printf(" angle=%8.4f ",angle);
+        pc.printf(" rate=%8.4f ",rate);
+        pc.printf(" x_acc=%8.4f ",x_acc);
+        pc.printf(" y_acc=%8.4f ",y_acc);
+        pc.printf(" z_acc=%8.4f ",z_acc);
+        pc.printf("\r\n");
+        
+        Vx = PID_x(aim_x);
+        Vy = PID_y(aim_y);
+        Rm = Rm(Vx,Vy);
+        Lm = Lm(Vx,Vy);
+        Tm = Tm(Vx,Vy);
+        if(Rm>0) {
+            RF = Rm;
+        } else {
+            RB = Rm *(-1);
+        }
+        if(Lm>0) {
+            LF = Lm;
+        } else {
+            LB = Lm *(-1);
+        }
+        if(Tm>0) {
+            TF = Tm;
+        } else {
+            TB = Tm *(-1);
+        }
+    }
+}
+void calcOmega()
+{
+    EC.calOmega();
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Sep 11 07:35:42 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file