Revision 9:92ddadd9e9a9, committed 2016-04-04

Comitter:

username16248

Date:

Mon Apr 04 07:20:24 2016 +0000

Parent:

8:554e3d643b2d

Child:

10:369a9389bb7e

Commit message:

???????????????

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/encoder.cpp	Mon Apr 04 07:20:24 2016 +0000
@@ -0,0 +1,145 @@
+#include "mbed.h"
+#include <pin_file.h>
+#include <encoder.h>
+InterruptIn b(EC1_b);//前部
+InterruptIn a(EC1_a);
+InterruptIn d(EC2_b);//右部
+InterruptIn c(EC2_a);
+InterruptIn e(EC3_a);//左部
+InterruptIn f(EC3_b);
+
+int count_f=0,count_r=0,count_l=0;
+int pa,pb,pC,pd,pe,pf;
+//int s=0;
+
+//interrupt
+void risea()
+{
+    pa=1;
+    if(pb==0) {
+        count_f++;
+    } else {
+        count_f--;
+    }
+}
+void falla()
+{
+    pa=0;
+    if(pb==0) {
+        count_f--;
+    } else {
+        count_f++;
+    }
+}
+void riseb()
+{
+    pb=1;
+    if(pa==0) {
+        count_f--;
+    } else {
+        count_f++;
+    }
+}
+void fallb()
+{
+    pb=0;
+    if(pa==0) {
+        count_f++;
+    } else {
+        count_f--;
+    }
+}
+
+
+void risec()
+{
+    pC=1;
+    if(pd==0) {
+        count_r++;
+    } else {
+        count_r--;
+    }
+}
+void fallc()
+{
+    pC=0;
+    if(pd==0) {
+        count_r--;
+    } else {
+        count_r++;
+    }
+}
+void rised()
+{
+    pd=1;
+    if(pC==0) {
+        count_r--;
+    } else {
+        count_r++;
+    }
+}
+void falld()
+{
+    pd=0;
+    if(pC==0) {
+        count_r++;
+    } else {
+        count_r--;
+    }
+}
+
+
+void risee()
+{
+    pe=1;
+    if(pf==0) {
+        count_l++;
+    } else {
+        count_l--;
+    }
+}
+void falle()
+{
+    pe=0;
+    if(pf==0) {
+        count_l--;
+    } else {
+        count_l++;
+    }
+}
+void risef()
+{
+    pf=1;
+    if(pe==0) {
+        count_l--;
+    } else {
+        count_l++;
+    }
+}
+void fallf()
+{
+    pf=0;
+    if(pe==0) {
+        count_l++;
+    } else {
+        count_l--;
+    }
+}
+
+void encoder_setting(){
+    //interrupt
+    a.rise(&risea);
+    a.fall(&falla);
+    b.rise(&riseb);
+    b.fall(&fallb);
+    c.rise(&risec);
+    c.fall(&fallc);
+    d.rise(&rised);
+    d.fall(&falld);
+    e.rise(&risee);
+    e.fall(&falle);
+    f.rise(&risef);
+    f.fall(&fallf);
+    //s++;
+    }
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/encoder.h	Mon Apr 04 07:20:24 2016 +0000
@@ -0,0 +1,13 @@
+//for encoder count
+#ifndef encoder_H
+#define encoder_H
+
+
+extern int count_f,count_r,count_l;
+extern int pa,pb,pC,pd,pe,pf;
+extern int s;
+
+
+#endif
+
+

--- a/pin_file.h	Sun Apr 03 18:30:55 2016 +0000
+++ b/pin_file.h	Mon Apr 04 07:20:24 2016 +0000
@@ -2,18 +2,30 @@
 #define PIN_FILE_H
 
 //速度制御
-InterruptIn a(PC_2);
-InterruptIn b(PC_3);
-InterruptIn c(PA_15);
-InterruptIn d(PC_13);
-InterruptIn e(PC_10);
-InterruptIn f(PC_12);
-PwmOut g(PB_8);
-PwmOut h(PB_9);
-PwmOut i(PB_6);
-PwmOut j(PA_9);
-PwmOut k(PA_8);
-PwmOut l(PB_10);
+#define EC1_a PC_2
+#define EC1_b PC_3
+#define EC2_a PA_15
+#define EC2_b PC_13
+#define EC3_a PC_10
+#define EC3_b PC_12
+//InterruptIn a(PC_2);
+//InterruptIn b(PC_3);
+//InterruptIn c(PA_15);
+//InterruptIn d(PC_13);
+//InterruptIn e(PC_10);
+//InterruptIn f(PC_12);
+#define WHf1 PB_8
+#define WHf2 PB_9
+#define WHr1 PB_6
+#define WHr2 PA_9
+#define WHl1 PA_8
+#define WHl2 PB_10
+//PwmOut g(PB_8);
+//PwmOut h(PB_9);
+//PwmOut i(PB_6);
+//PwmOut j(PA_9);
+//PwmOut k(PA_8);
+//PwmOut l(PB_10);
 //自己位置
 InterruptIn m(PC_7);
 InterruptIn n(PC_11);

--- /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);
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/speed_control.h	Mon Apr 04 07:20:24 2016 +0000
@@ -0,0 +1,28 @@
+//for speed control
+#ifndef SPEED_CONTROL_H
+#define SPEED_CONTROL_H
+
+extern float angle_f,angle_r,angle_l;
+extern float angle_old_f,angle_old_r,angle_old_l;
+extern float duty_out_f,duty_out_r,duty_out_l;
+extern float duty_PID_f,duty_PID_r,duty_PID_l;
+extern float omega_now_f,omega_now_r,omega_now_l;
+extern float rot_now_f,rot_now_r,rot_now_l;
+extern float rot_old_f,rot_old_r,rot_old_l;
+extern float rot_PID_f,rot_PID_r,rot_PID_l;
+extern float rot_prop_f,rot_prop_r,rot_prop_l;
+extern float rot_diff_f,rot_diff_r,rot_diff_l;
+extern float rot_inte_f,rot_inte_r,rot_inte_l;
+extern int dir_f,dir_r,dir_l;
+
+extern int i;
+//int targetA,targetB,targetC;
+extern int ka,kb,kc;
+extern int df,dr,dl;
+
+extern const float Kp,Kd,Ki;
+extern const float resolution;
+
+extern int t;
+
+#endif
\ No newline at end of file