Diff: speed_control.cpp

Revision:

9:92ddadd9e9a9

Child:

11:f8fac9693cd5

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/speed_control.cpp	Mon Apr 04 07:20:24 2016 +0000
@@ -0,0 +1,202 @@
+#include "mbed.h"
+#include <pin_file.h>
+#include <speed_control.h>
+#include <encoder.h>
+#define DUTY_MAX 0.9f
+
+PwmOut wheelf1(WHf1);//前部
+PwmOut wheelf2(WHf2);
+PwmOut wheelr1(WHr1);//右部
+PwmOut wheelr2(WHr2);
+PwmOut wheell1(WHl1);//左部
+PwmOut wheell2(WHl2);
+Ticker rot_cal;
+float angle_f=0.0,angle_r=0.0,angle_l=0.0;
+float angle_old_f=0.0,angle_old_r=0.0,angle_old_l=0.0;
+float duty_out_f=0.0,duty_out_r=0.0,duty_out_l=0.0;
+float duty_PID_f=0.0,duty_PID_r=0.0,duty_PID_l=0.0;
+float omega_now_f=0.0,omega_now_r=0.0,omega_now_l=0.0;
+float rot_now_f=0.0,rot_now_r=0.0,rot_now_l=0.0;
+float rot_old_f=0.0,rot_old_r=0.0,rot_old_l=0.0;
+float rot_PID_f=0.0,rot_PID_r=0.0,rot_PID_l=0.0;
+float rot_prop_f=0.0,rot_prop_r=0.0,rot_prop_l=0.0;
+float rot_diff_f=0.0,rot_diff_r=0.0,rot_diff_l=0.0;
+float rot_inte_f=0.0,rot_inte_r=0.0,rot_inte_l=0.0;
+int dir_f,dir_r,dir_l;
+const float Kp_f=30.0000f,Kd_f=200.0000f,Ki_f=0.0001f;
+const float Kp_r=30.0000f,Kd_r=200.0000f,Ki_r=0.0001f;
+const float Kp_l=30.0000f,Kd_l=200.0000f,Ki_l=0.0001f;
+const float resolution=1024.0;
+int t=0;
+/*
+void motor_move()
+{
+    wheelf1=0.1;
+    wheelf2=0;
+    wheelr1=0.1;
+    wheelr2=0;
+    wheell1=0.1;
+    wheell2=0;
+}
+*/
+
+
+////////////////////////////モーターそれぞれのPID制御/////////////////////////
+void motor_f(int dir_f,float rot_target_f)
+{
+    rot_prop_f=rot_target_f-rot_now_f;
+    //rot_diff_f=rot_now_f-rot_old_f;
+    rot_inte_f+=rot_prop_f;
+    rot_PID_f=Kp_f*rot_prop_f+Kd_f*rot_diff_f;//+Ki_f*rot_inte;
+    duty_PID_f=rot_PID_f*1.092f*0.0001f;
+    duty_out_f=duty_out_f+duty_PID_f;
+
+    if(duty_out_f>DUTY_MAX) {
+        duty_out_f=DUTY_MAX;
+    } else if(duty_out_f<0) {
+        duty_out_f=0;
+    }
+
+    if(dir_f==1) {
+        wheelf1=duty_out_f;
+        wheelf2=0;
+    } else if(dir_f==-1) {
+        wheelf1=0;
+        wheelf2=duty_out_f;
+    } else {
+        wheelf1=0;
+        wheelf2=0;
+    }
+}
+
+
+void motor_r(int dir_r,float rot_target_r)
+{
+    rot_prop_r=rot_target_r-rot_now_r;
+    //rot_diff_r=rot_now_r-rot_old_r;
+    rot_inte_r+=rot_prop_r;
+    rot_PID_r=Kp_r*rot_prop_r+Kd_r*rot_diff_r;//+Ki_r*rot_inte;
+    duty_PID_r=rot_PID_r*1.092f*0.0001f;
+    duty_out_r=duty_out_r+duty_PID_r;
+
+    if(duty_out_r>DUTY_MAX) {
+        duty_out_r=DUTY_MAX;
+    } else if(duty_out_r<0) {
+        duty_out_r=0;
+    }
+
+    if(dir_r==1) {
+        wheelr1=duty_out_r;
+        wheelr2=0;
+    } else if(dir_r==-1) {
+        wheelr1=0;
+        wheelr2=duty_out_r;
+    } else {
+        wheelr1=0;
+        wheelr2=0;
+    }
+}
+
+void motor_l(int dir_l,float rot_target_l)
+{
+    rot_prop_l=rot_target_l-rot_now_l;
+    //rot_diff_l=rot_now_l-rot_old_l;
+    rot_inte_l+=rot_prop_l;
+    rot_PID_l=Kp_l*rot_prop_l+Kd_l*rot_diff_l;//+Ki_l*rot_inte;
+    duty_PID_l=rot_PID_l*1.092f*0.0001f;
+    duty_out_l=duty_out_l+duty_PID_l;
+
+    if(duty_out_l>DUTY_MAX) {
+        duty_out_l=DUTY_MAX;
+    } else if(duty_out_l<0) {
+        duty_out_l=0;
+    }
+
+    if(dir_l==1) {
+        wheell1=duty_out_l;
+        wheell2=0;
+    } else if(dir_l==-1) {
+        wheell1=0;
+        wheell2=duty_out_l;
+    } else {
+        wheell1=0;
+        wheell2=0;
+    }
+}
+///////////////////////////////////////////////////////////////
+
+void motor_target(double rot_target_f,double rot_target_r,double rot_target_l)
+{
+    ////////////////////前、回転方向決定/////////////////////
+    if(rot_target_f>0) {
+        dir_f=1;
+    } else if(rot_target_f<0) {
+        dir_f=-1;
+        rot_target_f=-rot_target_f;
+    } else {
+        dir_f=0;
+    }
+    motor_f(dir_f,rot_target_f);
+    ////////////////////右、回転方向決定/////////////////////
+    if(rot_target_r>0) {
+        dir_r=1;
+    } else if(rot_target_r<0) {
+        dir_r=-1;
+        rot_target_r=-rot_target_r;
+    } else {
+        dir_r=0;
+    }
+    motor_r(dir_r,rot_target_r);
+    ////////////////////左、回転方向決定/////////////////////
+    if(rot_target_l>0) {
+        dir_l=1;
+    } else if(rot_target_l<0) {
+        dir_l=-1;
+        rot_target_l=-rot_target_l;
+    } else {
+        dir_l=0;
+    }
+    motor_l(dir_l,rot_target_l);
+}
+
+
+//////////////////////////////////////////////////////////////////////
+//////////////////////////////////回転数計算/////////////////////////////////
+void rotation_cal()
+{
+    ////////////////////////motor_f////////////////////////
+    angle_f=count_f/resolution*2.0f*3.14f;
+    omega_now_f=abs(angle_f-angle_old_f)/0.01f;
+    rot_now_f=omega_now_f*10/(2.0f*3.14f);
+    rot_diff_f=rot_now_f-rot_old_f;
+    angle_old_f=angle_f;
+    rot_old_f=rot_now_f;
+    ////////////////////////motor_r////////////////////////
+    angle_r=count_r/resolution*2.0f*3.14f;
+    omega_now_r=abs(angle_r-angle_old_r)/0.01f;
+    rot_now_r=omega_now_r*10/(2.0f*3.14f);
+    rot_diff_r=rot_now_r-rot_old_r;
+    angle_old_r=angle_r;
+    rot_old_r=rot_now_r;
+    ////////////////////////motor_l////////////////////////
+    angle_l=count_l/resolution*2.0f*3.14f;
+    omega_now_l=abs(angle_l-angle_old_l)/0.01f;
+    rot_now_l=omega_now_l*10/(2.0f*3.14f);
+    rot_diff_l=rot_now_l-rot_old_l;
+    angle_old_l=angle_l;
+    rot_old_l=rot_now_l;
+    //t++;
+}
+
+void motor_setting()
+{
+    //////////////////////////////モーターの設定をここに書いてください////////////////////////
+    wheelf1.period_us(50);
+    wheelf2.period_us(50);
+    wheelr1.period_us(50);
+    wheelr2.period_us(50);
+    wheell1.period_us(50);
+    wheell2.period_us(50);
+    rot_cal.attach(&rotation_cal,0.001);
+}
+