File content as of revision 14:3403ce49a37a:

#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);
}