File content as of revision 4:51b3e070cd89:

#include "mbed.h"
#include "EC.h"
#define RESOLUTION 500
#include "CalPID.h"
#include "MotorController.h"
#include <ros.h>
#include <geometry_msgs/Point.h>
#define DELTA_T 0.001

#define TIME_TURNING 0.8
#define DUTY_MAX 0.8
#define OMEGA_MAX 5

////////////////////////////////////////
#define THROW_SPEED 11.9

///////////////////////////////////////
#define THROW_FIN 95
#define BOTTOM_ANGLE 0.5
#define STOP_ANGLE 0.1


DigitalIn toggle(p6,PullUp);

Timer timer;
Timer timer_loop;
Ticker ticker;
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);

//CalPID speed_pid(0.9281,0,0.0002486,DELTA_T,DUTY_MAX);
//CalPID angle_duty_pid(0,0,0,DELTA_T,DUTY_MAX);
//CalPID angle_omega_pid(6.726,0,0.0007063,DELTA_T,OMEGA_MAX);

CalPID speed_pid(0.9281,0,0.0003086,DELTA_T,DUTY_MAX);
CalPID angle_duty_pid(0,0,0,DELTA_T,DUTY_MAX);
CalPID angle_omega_pid(6.726,0,0.0007063,DELTA_T,OMEGA_MAX);


Ec1multi ec(p17,p18,RESOLUTION);  //2逓倍用class
MotorController motor(p21,p22,DELTA_T,ec,speed_pid,angle_duty_pid,angle_omega_pid);

float shot = 0;

void throwCallback(const geometry_msgs::Point &throw_state){
    shot = throw_state.x; //1になったら発射
}

double convertRad(double degree)
{
    return degree*M_PI/180.0;
}

float data_saved[2500]= {};
int data_count=0;
void saveData()
{
    if(data_count<150) {
        data_saved[data_count]=ec.getDeg();
        data_count++;
    }
}
float target_rad=0;
float target_speed=0;
float omega_saved[2500]= {};
int omega_count=0;
void saveOmega()
{
    if(omega_count<150) {
        omega_saved[omega_count]=ec.getOmega();
        omega_count++;
    }
}

void setSpeed(double target_rads)
{
    motor.setPDParamSc(0.9281,0.0003086);
    target_speed=target_rads;
}
void setAngle(double target_deg)
{
    motor.setPDParamSc(0.00100,0.00000070000);
    target_rad=convertRad(target_deg);
}

void speedControll()
{
    motor.Sc(target_speed);
//    saveData();
//    saveOmega();
}
void angleControll()
{
    motor.AcOmega(target_rad);
//    saveData();
//    saveOmega();
}

void displayData()
{
    for(int i=0; i<omega_count; i++) {
        printf("%f\t%f\t%f\r\n",i*DELTA_T,data_saved[i],omega_saved[i]);
        wait(0.01);
    }
    omega_count=0;
    data_count=0;
}

double initial_duty=0;
void inputDuty()
{
    motor.turn(initial_duty);
    ec.calOmega();
    saveData();
    saveOmega();
}
void initialThrow() //バックラッシュ対策
{
    double dead_time=0;
//    if(target_speed<9) {
//        initial_duty=0.4;
//        motor.turn(initial_duty);
//        dead_time=0.020;
//    } else if(target_speed<13) {
//        initial_duty=0.30;
//        motor.turn(initial_duty);
//        dead_time=0.0200;
//    } else if(target_speed<18) {
//        initial_duty=0.30;
//        motor.turn(initial_duty);
////        dead_time=0.024;
//        dead_time=0.020;
//    }  else if(target_speed<20) {
//        initial_duty=0.63;
//        motor.turn(initial_duty);
//        dead_time=0.020;
//    } else if(target_speed<21) {
//        initial_duty=0.63;
//        motor.turn(initial_duty);
//        dead_time=0.031;
//    } else if(target_speed<23) {
//        initial_duty=0.64;
//        motor.turn(initial_duty);
//        dead_time=0.030;
//    } else if(target_speed<25) {
//        initial_duty=0.70;
//        motor.turn(initial_duty);
//        dead_time=0.030;
//    } else if(target_speed<27) {
//        initial_duty=0.75;
//        motor.turn(initial_duty);
//        dead_time=0.030;
//    } else if(target_speed<29) {
//        initial_duty=0.87;
//        motor.turn(initial_duty);
//        dead_time=0.030;
//    }

    initial_duty=0.05;
    dead_time=0.30;
    ticker.attach(&inputDuty,DELTA_T);
    wait(dead_time);
    ticker.detach();
}

ros::NodeHandle nh;
ros::Subscriber<geometry_msgs::Point> sub("/throw",&throwCallback);

int main(){
    nh.getHardware()->setBaud(115200);
    nh.initNode();
    nh.subscribe(sub);
    NVIC_SetPriority(TIMER3_IRQn, 5);
/////////////////////////////////////////////////////////////////////////////

    motor.setEquation(0.0226,0.0039,-0.0226,0.0044);//MAXON_17.2

//    while(1) {
//        motor.turn(0.1);
//        printf("%d\r\n",ec.getCount());
//        wait(0.5);
//    }
//////////////////////////////////////////////////////////////////////////////

    int state=0;

    //printf("READY!!\r\n");
    led2=1;

    while(1) {
        nh.spinOnce();      //ROS
        if(state==0) {
            if(shot == 1) {
                led3 = 1;
                state++;
                motor.reset();
                setSpeed(THROW_SPEED);
                initialThrow();
                ticker.attach(&speedControll,DELTA_T);
            } else {
                led1=!led1;
                wait(0.5);
            }
        } else if(state==1) {
            if(ec.getDeg()>THROW_FIN) {
                ticker.detach();
                setAngle(THROW_FIN+10);
                ticker.attach(&angleControll,DELTA_T);
                state++;
                timer.reset();
                timer.start();
            }
        }  else if(state==2) {
            if(fabs(ec.getDeg()-(THROW_FIN+10))>2.5) {
                timer.reset();
            } else if(timer.read()>1.30) {
                ticker.detach();
                state++;
            }
        } else if(state==3) {
            setAngle(BOTTOM_ANGLE);
            ticker.attach(&angleControll,DELTA_T);
            state++;
        } else if(state==4) {
            if(fabs(ec.getDeg()-BOTTOM_ANGLE)>1) {
                timer.reset();
            } else if(timer.read()>0.05) {
                ticker.detach();
                state++;

            }
        } else if(state==5) {
            state++;
            motor.stop();

        } else if(state==6) {
            displayData();
            state++;
        } else if(state==7) {
//            state=0;
            state++;
            wait(2);
        }
        wait_us(10);

    }
}