Revision 0:fb4269aa5fb4, committed 2017-05-26

Comitter:

PicYusuke

Date:

Fri May 26 03:51:19 2017 +0000

Commit message:

hoge

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Robot/inc/filter.h	Fri May 26 03:51:19 2017 +0000
@@ -0,0 +1,15 @@
+#ifndef __filter_H
+#define __filter_H
+
+#include <stdint.h>
+
+typedef struct{
+    
+    float u_buf[2];
+    float y_buf[2];
+    
+} filter_struct;
+
+float iir(float, filter_struct *);
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Robot/inc/motor.h	Fri May 26 03:51:19 2017 +0000
@@ -0,0 +1,10 @@
+#ifndef __motor_H
+#define __motor_H
+
+#include "tim.h"
+
+void Put_Vm_L(float);
+void Put_Vm_R(float);
+void Put_Vm_Lift(float);
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Robot/inc/pid.h	Fri May 26 03:51:19 2017 +0000
@@ -0,0 +1,38 @@
+/* 
+ * File:   pid.h
+ * Author: Owner
+ *
+ * Created on 2016/02/29, 1:12
+ */
+
+#ifndef PID_H
+#define	PID_H
+
+#include <stdint.h>
+
+#define PID_CONTROL_ENABLE      1
+#define PID_CONTROL_DISABLE     0
+
+typedef struct{
+        
+	float kp;
+	float kd;
+	float ki;
+	float diff[2];
+	float i_term;
+	
+	uint32_t is_enabled;
+	uint32_t is_initialized;
+
+} PID_Struct;
+
+void PID_Struct_Init(PID_Struct *, float, float, float);
+void PID_Enable_Control(PID_Struct *);
+void PID_Disable_Control(PID_Struct *);
+uint32_t PID_Check_Status(PID_Struct *);
+float PID_Control(PID_Struct *, float, float);
+float PD_Control(PID_Struct *, float, float);
+float PI_Control(PID_Struct *, float, float);
+
+#endif	/* PID_H */
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Robot/inc/robot.h	Fri May 26 03:51:19 2017 +0000
@@ -0,0 +1,13 @@
+#ifndef ROBOT_H
+#define ROBOT_H
+
+#include <math.h>
+
+#include "motor.h"
+#include "filter.h"
+#include "pid.h"
+
+#define PI  3.1415926535f
+#define INV_PI ((float)(1.0f / 3.1415926535f))
+
+#endif  /* ROBOT_H */
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Robot/src/filter.cpp	Fri May 26 03:51:19 2017 +0000
@@ -0,0 +1,12 @@
+#include "filter.h"
+
+float iir(float uk, filter_struct *flt)
+{
+	const float coef = 0.3f;
+	float yk;
+	
+	yk = coef * uk + (1.0f - coef) * flt->y_buf[0];
+	flt->y_buf[0] = yk;
+	
+	return yk;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Robot/src/motor.cpp	Fri May 26 03:51:19 2017 +0000
@@ -0,0 +1,57 @@
+#include "motor.h"
+
+float v_bat = 11.3f;     //モータ用電源電圧
+
+/*-----使用するPWMモジュール-----*/
+PwmOut *motor_L = &pwm2;
+PwmOut *motor_R = &pwm1;
+PwmOut *motor_lift = &pwm3;
+/*----------------------------*/
+
+/*--------回転方向制御I/O--------*/
+DigitalOut ml_dir(PB_3);    //left motor
+DigitalOut mr_dir(PB_4);    //right motor
+DigitalOut me_dir(PB_10);    //lift motor (elevation)
+/*-----------------------------*/
+
+void Put_Vm_L(float v)
+{
+    if(v < 0.0f)
+    {
+        ml_dir = 1;
+        (*motor_L).write(-v / v_bat);
+    }
+    else
+    {
+        ml_dir = 0;
+        (*motor_L).write(v / v_bat);
+    }
+}
+
+void Put_Vm_R(float v)
+{
+    if(v < 0.0f)
+    {
+        mr_dir = 1;
+        (*motor_R).write(-v / v_bat);
+    }
+    else
+    {
+        mr_dir = 0;
+        (*motor_R).write(v / v_bat);
+    }
+}
+
+void Put_Vm_Lift(float v)
+{
+    if(v > 0.0f)
+    {
+        me_dir = 1;
+        (*motor_lift).write(v / v_bat);
+    }
+    else
+    {
+        me_dir = 0;
+        (*motor_lift).write(-v / v_bat);
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Robot/src/pid.cpp	Fri May 26 03:51:19 2017 +0000
@@ -0,0 +1,60 @@
+#include "pid.h"
+
+
+void PID_Struct_Init(PID_Struct *pid, float Kp, float Kd, float Ki)
+{
+    pid->kp = Kp;
+    pid->kd = Kd;
+    pid->ki = Ki;
+    pid->diff[0] = 0;
+    pid->diff[1] = 0;
+    pid->i_term = 0;
+    
+    pid->is_initialized = 1;
+    pid->is_enabled = PID_CONTROL_DISABLE;
+}
+
+void PID_Enable_Control(PID_Struct *pid)
+{
+    pid->is_enabled = PID_CONTROL_ENABLE;
+}
+
+void PID_Disable_Control(PID_Struct *pid)
+{
+    pid->is_enabled = PID_CONTROL_DISABLE;
+}
+
+uint32_t PID_Check_Status(PID_Struct *pid)
+{
+    return pid->is_enabled;
+}
+
+float PID_Control(PID_Struct *pid, float new_input, float target)
+{
+    pid->diff[1] = -pid->diff[0];               //diff[1] = -e(k-1)
+    pid->diff[0] = target - new_input;      //e(k) = target - input
+    pid->diff[1] += pid->diff[0];             //diff[1] = diff[1] + e(k)
+                                                        //Eventually, diff[1] = e(k) - e(k-1)
+    pid->i_term += pid->ki * pid->diff[0];
+    
+    return (pid->kp * pid->diff[0] + pid->kd * pid->diff[1] + pid->i_term);
+}
+
+/*---------------These functions below are faster than PID_Control()----------------*/
+float PD_Control(PID_Struct *pid, float new_input, float target)
+{
+    pid->diff[1] = -pid->diff[0];               //diff[1] = -e(k-1)
+    pid->diff[0] = target - new_input;      //e(k) = target - input
+    pid->diff[1] += pid->diff[0];             //diff[1] = diff[1] + e(k)
+                                                        //Eventually, diff[1] = e(k) - e(k-1)
+    return (pid->kp * pid->diff[0] + pid->kd * pid->diff[1]);
+}
+
+float PI_Control(PID_Struct *pid, float new_input, float target)
+{
+    pid->diff[0] = target - new_input;      //e(k) = target - input
+    pid->i_term += pid->ki * pid->diff[0];
+    
+    return (pid->kp * pid->diff[0] + pid->i_term);
+}
+/*---------------------------------------------------------------------------*/

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri May 26 03:51:19 2017 +0000
@@ -0,0 +1,855 @@
+#include "mbed.h"
+
+#include "gpio.h"
+
+#include "robot.h"
+#include "GP2Y0E03.h"
+#include "MCP3208.h"
+#include "L3GD20.h"
+
+#define ERROR   (-1)
+#define SUCCESS 0
+
+//#define L3GD20_TEST
+//#define GP2Y0E03_TEST
+//#define MCP3208_TEST
+//#define LBR127_TEST
+
+/*---------RCサーボ制御用--------*/
+float servo_pulse = 0.001f;     //RCサーボパルス幅[s]
+uint32_t servo_state = 0;       //サーボピン状態
+/*-----------------------------*/
+
+/*-----------------ラインセンサ--------------------*/
+float line_pos[2] = {0.0f, 0.0f};
+uint32_t intersection = 0;          //交差点検知
+/*------------------------------------------------*/
+
+/*---------状態変数----------*/
+float v_tr = 0.0f;      //並進方向電圧, unit: V
+float theta = 0.0f;     //姿勢角, unit: degree
+
+float omega_d = 0.0f;   //目標角速度, unit: degree/s
+/*--------------------------*/
+
+/*-----------------------------*/
+float ts = 0.001f;                  //制御周期
+
+uint32_t sequence       = 0;       //タスク管理変数
+uint32_t error_seq      = 0xFF;        //エラーを起こしているタスク番号
+int32_t (*Task_Fptr[10])(void);     //タスク管理用関数ポインタ
+/*-----------------------------*/
+
+/*-----制御器-----*/
+PID_Struct trace_pid;   //ライントレース用
+PID_Struct gyro_pid;   //ジャイロによるオドメトリ用
+
+PID_Struct manual_control; //手動モータ制御用
+
+PID_Struct *current_pid_ptr;   //現在接続されている制御器
+/*----------------*/
+
+/*---------各タスク関数--------*/
+//ロボットの動作をシーケンシャルに記述する
+//各タスク関数内ではフィードバック制御則へのアクセス原則禁止
+int32_t Task0(void);   
+int32_t Task1(void);
+int32_t Task2(void);
+int32_t Task3(void);
+int32_t Task4(void);
+int32_t Task5(void);   
+int32_t Task6(void);
+int32_t Task7(void);
+int32_t Task8(void);
+int32_t Task9(void);
+/*---------------------------*/
+
+void Turn_90deg(int32_t);
+void Turn_180deg(int32_t);
+
+void Accelerate(float);
+void Decelerate();
+void Stop();
+
+int main() 
+{
+    uint32_t i;
+    
+    /*Peripheral initialization*/
+    //GPIO_Init();
+    
+    SPI_Init();
+    
+    /*-----各種デバイス初期化----*/
+    L3GD20_Init();
+    MCP3208_Init();
+    /*-------------------------*/
+    
+    /*------フィードバックコントローラ-------*/
+    PID_Struct_Init(&trace_pid, 1.1, 20.0, 0.0);
+    //PID_Struct_Init(&gyro_pid, 0.015, 0.0, 0.0);
+    PID_Struct_Init(&gyro_pid, 0.02, 0.0, 0.0);
+    current_pid_ptr = &manual_control;
+    /*-----------------------------------*/
+    
+    TIM_Init();     //タイマスタート
+    /*-------------------------*/
+    
+    /*-----関数ポインタ設定------*/
+    Task_Fptr[0] = Task0;
+    Task_Fptr[1] = Task1;
+    Task_Fptr[2] = Task2;
+    Task_Fptr[3] = Task3;
+    Task_Fptr[4] = Task4;
+    Task_Fptr[5] = Task5;
+    Task_Fptr[6] = Task6;
+    Task_Fptr[7] = Task7;
+    Task_Fptr[8] = Task8;
+    Task_Fptr[9] = Task9;
+    /*-------------------------*/
+    
+    
+    
+    
+    while(1)
+    {
+        if(Task_Fptr[sequence]() == ERROR)  //タスク呼び出し, エラーチェック
+        {
+            error_seq = sequence;       //エラーを起こしているタスク番号を取得
+        }
+    
+        sequence ++;
+        /*
+        if(sequence == 1)
+        {
+            sequence = 3;
+        }
+        */
+    }
+}
+
+int32_t Task0(void)
+{
+    /*
+    if()
+    {
+        return ERROR;   
+    }
+    */
+    
+    servo_pulse = 0.0021f;  //アームを折りたたむ
+    wait(0.5f);
+    
+    /*-----最下点までリフトを移動-----*/
+    Put_Vm_Lift(-0.8f);      //リフト下降
+    wait(2.0f);
+    Put_Vm_Lift(0.0f);      //リフト停止
+    /*-----------------------------*/
+    /*----リフトのリポジショニング----*/
+    Put_Vm_Lift(1.0f);      //リフト上昇(どん兵衛用)
+    wait(0.8f);
+    Put_Vm_Lift(0.0f);      //リフト停止
+    /*-----------------------------*/
+    
+    /*--------------------スタート待機-----------------------*/
+    while(ain_cup > 0.5f) //カップ検出センサが反応するまで待機
+    {
+        printf("cup = %f\n", (float)ain_cup);
+    }
+    
+    servo_pulse = 0.001f;  //アームを展開
+    wait(0.3f);
+    /*-----------------------------------------------------*/
+    /*
+    while(1)
+    {
+        HAL_Delay(10);
+        printf("pos = %f\n", line_pos[0]);
+    }
+    */
+    return 0;
+}
+
+int32_t Task1(void)
+{
+    uint32_t i;
+    float v = 0.0f;
+    
+    /*
+    if()
+    {
+        return ERROR;   
+    }
+    */
+    
+    /*-------------------ライントレース--------------------*/
+    current_pid_ptr = &trace_pid;   //制御器変更
+    Accelerate(2.0f);       //加速
+    
+    for(i = 0; i < 2; i ++)     //1-2回目の交差点はスキップ
+    {
+        while(!intersection)    //一回目の交差点検出はスキップ
+        {
+            printf("running...\n");   
+        }
+        while(intersection)    //一回目の交差点通過を待つ
+        {
+            printf("running...\n");   
+        }
+    }
+    /*-----------------------------------------------------*/
+    
+    return 0;
+}
+
+int32_t Task2(void)     //3本目の交差点を左折
+{
+    /*
+    if()
+    {
+        return ERROR;   
+    }
+    */
+    
+    while(!intersection)    //3回目の交差点検出まで直進
+    {
+        printf("running...\n");   
+    }
+    while(intersection)    //3回目の交差点通過を待つ
+    {
+        printf("running...\n");   
+    }
+    
+    wait(0.4f);
+    
+    Stop();         //停止
+    Turn_90deg(1);  //左90度旋回
+    
+    return 0;
+}
+
+int32_t Task3(void)     //横道を直進し、どん兵衛を回収、180度回転
+{
+    uint32_t i;
+    float v = 0.0f;
+    
+    /*
+    if()
+    {
+        return ERROR;   
+    }
+    */
+    
+    /*-------------------ライントレース--------------------*/
+    current_pid_ptr = &trace_pid;   //制御器変更
+    Accelerate(1.0f);       //加速
+    /*-----------------------------------------------------*/
+    
+    #if 0
+    //本来はここでカップを検出するまで前進する
+    wait(1.3f);
+    #endif
+    
+    #if 1
+    while(ain_cup > 0.6f) //カップ検出まで前進
+    {
+        printf("waiting...\n");
+    }
+    
+    wait(0.05f);
+    Stop();                 //停止
+    
+    servo_pulse = 0.0021f;  //カップ把持
+    wait(1.0f);
+    
+    Put_Vm_Lift(1.5f);      //リフト上昇
+    wait(0.8f);
+    Put_Vm_Lift(0.0f);      //リフト停止
+    #endif
+    
+    current_pid_ptr = &trace_pid;   //制御器変更
+    Accelerate(1.5f);       //加速
+    wait(0.5f);
+    Stop();                 //停止
+    
+    Turn_180deg(1); //左180度旋回
+    
+    return 0;
+}
+
+int32_t Task4(void) //カップ回収後、メインストリートに戻る
+{
+    uint32_t i;
+    float v = 0.0f;
+    
+    /*
+    if()
+    {
+        return ERROR;   
+    }
+    */
+    
+    /*-------------------ライントレース--------------------*/
+    current_pid_ptr = &trace_pid;   //制御器変更
+    Accelerate(1.5f);       //加速
+    /*-----------------------------------------------------*/
+    
+    while(!intersection)    //交差点検出まで直進
+    {
+        printf("running...\n");   
+    }
+    while(intersection)    //交差点通過を待つ
+    {
+        printf("running...\n");   
+    }
+    
+    wait(0.6f);
+    
+    Stop();
+    
+    Turn_90deg(1);  //左90度旋回
+
+    return 0;
+}
+
+int32_t Task5(void) //カップ回収後のメインストリート走行+カップ棚置き
+{
+    uint32_t i;
+    float v = 0.0f;
+    
+    /*
+    if()
+    {
+        return ERROR;   
+    }
+    */
+    /*-------------------ライントレース--------------------*/
+    current_pid_ptr = &trace_pid;   //制御器変更
+    Accelerate(2.0f);       //加速
+    /*-----------------------------------------------------*/
+    
+    while(ain_cup > 0.7f) //棚検出まで前進
+    {
+        printf("waiting...\n");
+    }
+    
+    Stop();
+    
+    servo_pulse = 0.001f;  //カップ解放
+    wait(0.5f);
+    
+    /*----------ジャイロを使って後退--------------*/
+    current_pid_ptr = &gyro_pid;   //制御器変更
+    Accelerate(-1.5f);       //加速
+    
+    wait(0.6f);
+    
+    Stop();
+    /*------------------------------------------*/
+    
+    Turn_180deg(1);  //左180度旋回
+    
+    /*-----最下点までリフトを移動-----*/
+    Put_Vm_Lift(-0.8f);      //リフト下降
+    wait(2.5f);
+    Put_Vm_Lift(0.0f);      //リフト停止
+    /*-----------------------------*/
+    /*----リフトのリポジショニング----*/
+    Put_Vm_Lift(1.0f);      //リフト上昇(UFO用)
+    wait(0.35f);
+    Put_Vm_Lift(0.0f);      //リフト停止
+    /*-----------------------------*/
+    
+    return 0;
+}
+
+int32_t Task6(void)     //4本目の交差点を棚側から左折
+{
+    
+    uint32_t i;
+    float v = 0;
+    /*
+    if()
+    {
+        return ERROR;   
+    }
+    */
+    
+    /*-------------------ライントレース--------------------*/
+    current_pid_ptr = &trace_pid;   //制御器変更
+    Accelerate(1.5f);       //加速
+    /*-----------------------------------------------------*/
+    
+    while(!intersection)    //交差点検出まで直進
+    {
+        printf("running...\n");   
+    }
+    while(intersection)    //交差点通過を待つ
+    {
+        printf("running...\n");   
+    }
+    
+    wait(0.6f);
+    
+    Stop();         //停止
+    Turn_90deg(1);  //左90度旋回
+    
+    return 0;
+}
+
+int32_t Task7(void)     //横道を直進し、UFOを回収、180度回転
+{
+    /*-------------------ライントレース--------------------*/
+    current_pid_ptr = &trace_pid;   //制御器変更
+    Accelerate(1.0f);       //加速
+    /*-----------------------------------------------------*/
+   
+    
+    while(ain_cup > 0.6f) //カップ検出まで前進
+    {
+        printf("waiting...\n");
+    }
+    
+    wait(0.05f);
+    Stop();                 //停止
+    
+    servo_pulse = 0.0021f;  //カップ把持
+    wait(1.0f);
+    
+    Put_Vm_Lift(1.5f);      //リフト上昇
+    wait(0.8f);
+    Put_Vm_Lift(0.0f);      //リフト停止
+    
+    
+    current_pid_ptr = &trace_pid;   //制御器変更
+    Accelerate(1.5f);       //加速
+    wait(0.5f);
+    Stop();                 //停止
+    
+    Turn_180deg(1); //左180度旋回
+}
+
+
+int32_t Task8(void)     //棚左側にUFOを置くために、位置合わせ
+{
+    uint32_t i;
+    float v = 0;
+    /*
+    if()
+    {
+        return ERROR;   
+    }
+    */
+    
+    /*-------------------ライントレース--------------------*/
+    current_pid_ptr = &trace_pid;   //制御器変更
+    Accelerate(1.5f);       //加速
+    /*-----------------------------------------------------*/
+    
+    /*---------------1回目の交差点-----------------*/
+    while(!intersection)    //交差点検出まで直進
+    {
+        printf("running...\n");   
+    }
+    while(intersection)    //交差点通過を待つ
+    {
+        printf("running...\n");   
+    }
+    
+    wait(2.5f); //棚左側とカップの位置を合わせるためのパラメータ
+    /*--------------------------------------------*/
+    
+    Stop();
+    Turn_90deg(-1); //右90度旋回
+    
+    return 0;
+}
+
+int32_t Task9(void)     //ジャイロを使って棚まで接近、UFOを置く
+{
+    uint32_t i;
+    float v = 0;
+    /*
+    if()
+    {
+        return ERROR;   
+    }
+    */
+    
+    /*----------ジャイロを使って前進--------------*/
+    current_pid_ptr = &gyro_pid;   //制御器変更
+    Accelerate(1.5f);       //加速
+    /*------------------------------------------*/
+    
+    while(ain_cup > 0.7f) //棚検出まで前進
+    {
+        printf("waiting...\n");
+    }
+    
+    Stop();
+    
+    servo_pulse = 0.001f;  //カップ解放
+    wait(0.5f);
+    
+    while(1)
+    {
+        printf("Tasks completed\n");
+    }
+    
+    
+    return 0;
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+#if 0
+
+int32_t Task8(void)     //棚左側にUFOを置くために、位置合わせ
+{
+    uint32_t i;
+    float v = 0;
+    /*
+    if()
+    {
+        return ERROR;   
+    }
+    */
+    
+    /*-------------------ライントレース--------------------*/
+    current_pid_ptr = &trace_pid;   //制御器変更
+    Accelerate(2.0f);       //加速
+    
+    for(i = 0; i < 3; i ++)     //1-3回目の交差点はスキップ
+    {
+        while(!intersection)    //一回目の交差点検出はスキップ
+        {
+            printf("running...\n");   
+        }
+        while(intersection)    //一回目の交差点通過を待つ
+        {
+            printf("running...\n");   
+        }
+    }
+    /*-----------------------------------------------------*/
+    
+    /*---------------4回目の交差点-----------------*/
+    while(!intersection)    //交差点検出まで直進
+    {
+        printf("running...\n");   
+    }
+    while(intersection)    //交差点通過を待つ
+    {
+        printf("running...\n");   
+    }
+    
+    wait(1.5f);
+    /*--------------------------------------------*/
+    
+    Stop();
+    
+    return 0;
+}
+
+int32_t Task9(void)
+{
+    uint32_t i;
+    float v = 0;
+    /*
+    if()
+    {
+        return ERROR;   
+    }
+    */
+    
+    while(1)
+    {
+        printf("Tasks completed\n");
+    }
+    
+    
+    return 0;
+}
+
+#endif
+
+void Turn_90deg(int32_t dir)
+{
+    uint32_t i;
+    float v = 0.0f;
+    float sign = 1.0f;
+    
+    if(dir == 1)    //回転方向
+    {
+        sign = 1.0f;
+    }
+    else if(dir == -1)
+    {
+        sign = -1.0f;
+    }
+    
+    theta = 0.0f;   //角度初期化
+    
+    /*--------台形加速(角度)---------*/
+    for(i = 0; i < 10; i ++)
+    {
+        v += 0.15f;
+        Put_Vm_L(-sign * v); //回転
+        Put_Vm_R(sign * v);
+        wait(0.05f);
+    }
+    /*-----------------------------*/
+
+    if(dir == 1)
+    { 
+        while(theta < 80.0f)
+        {
+            printf("Turning...\n");   
+        }
+    }
+    else if(dir == -1)
+    {
+        while(theta > -80.0f)
+        {
+            printf("Turning...\n");   
+        }
+    }
+    
+    Put_Vm_L(0.0f); //停止
+    Put_Vm_R(0.0f);
+    wait(0.5f);
+}
+
+void Turn_180deg(int32_t dir)
+{
+    uint32_t i;
+    float v = 0.0f;
+    float sign = 1.0f;
+    
+    if(dir == 1)    //回転方向
+    {
+        sign = 1.0f;
+    }
+    else if(dir == -1)
+    {
+        sign = -1.0f;
+    }
+    
+    theta = 0.0f;   //角度初期化
+    
+    /*--------台形加速(角度)---------*/
+    for(i = 0; i < 10; i ++)
+    {
+        v += 0.15f;
+        Put_Vm_L(-sign * v); //回転
+        Put_Vm_R(sign * v);
+        wait(0.05f);
+    }
+    /*-----------------------------*/
+
+    while(theta < 170.0f)
+    {
+        printf("Turning...\n");   
+    }
+    Put_Vm_L(0.0f); //停止
+    Put_Vm_R(0.0f);
+    wait(0.5f);
+}
+
+void Accelerate(float v_desired)    //v_desired[V]まで緩やかに加速
+{
+    uint32_t i;
+    float v = 0.0f;
+    float v_step = v_desired / 10.0f;   //単位速度
+
+    for(i = 0; i < 10; i ++)
+    {
+        v += v_step;
+        v_tr = v;
+        wait(0.05f);
+    }
+}
+
+void Decelerate()           //緩やかに減速、停止
+{
+    uint32_t i;
+    float v = 0.0f;
+    float v_step = -v_tr / 10.0f;
+
+    for(i = 0; i < 10; i ++)
+    {
+        v += v_step;
+        v_tr = v;
+        wait(0.05f);
+    }
+
+    v_tr = 0.0f;    //確実に停止
+    wait(0.1f);
+}
+
+void Stop()         //急激に減速、停止
+{
+    current_pid_ptr = &manual_control;  //制御OFF
+        Put_Vm_L(0.0f); 
+        Put_Vm_R(0.0f);
+        wait(0.5f);
+}
+
+void Flip1_Callback()       //RCサーボ制御用割り込み関数
+{
+    switch(servo_state)
+    {
+        case 0:
+            flipper1.attach(&Flip1_Callback, servo_pulse);
+            servo = 1;
+            servo_state ++;
+            break;
+        case 1:
+            flipper1.attach(&Flip1_Callback, 0.01f - servo_pulse);
+            servo = 0;
+            servo_state ++;
+            break;
+        default:
+            servo_state = 0;
+            break;
+    }
+}
+
+void Flip2_Callback()       //フィードバック制御用割り込み関数
+{
+    int32_t i;
+    uint32_t adres_max = 0;
+    uint32_t adres_min = 65535;  
+    uint32_t adres_mid = 0;
+    int32_t pos_int = 0;
+    
+    float omega = 0.0f;    //unit: deg/s
+    float vm_L, vm_R;       //unit: V
+    
+    /*-----------------センサ読み取り-----------------*/
+    //MCP3208_Read(0);
+    MCP3208_Read(1);
+    MCP3208_Read(2);
+    MCP3208_Read(3);
+    MCP3208_Read(4);
+    MCP3208_Read(5);
+    
+    L3GD20_Read();
+    /*-----------------------------------------------*/
+    
+    /*---------------最大最小値を求める----------------*/
+    for(i = 0; i < 5; i ++)
+    {
+        mcp3208.adres[i+1] = 4095 - mcp3208.adres[i+1];     //ADCの出力とラインの色に合わせて反転/非反転
+        
+        if(adres_min > mcp3208.adres[i+1])
+            adres_min = mcp3208.adres[i+1];
+        if(adres_max < mcp3208.adres[i+1])
+            adres_max = mcp3208.adres[i+1];
+    }        
+    
+    adres_mid = (adres_max + adres_min) / 2;
+    /*-----------------------------------------------*/
+    if(adres_max - adres_min > 200)
+    {
+        line_pos[0] = 0.0f;
+        for(i = 0; i < 5; i ++)
+        {
+            pos_int += (i+1)*(mcp3208.adres[i+1] - adres_min);
+        }
+        line_pos[0] = ((float)pos_int);
+        line_pos[0] = line_pos[0] / (mcp3208.adres[1]+mcp3208.adres[2]+mcp3208.adres[3]+mcp3208.adres[4]+mcp3208.adres[5]-5*adres_min) - 3.0f;
+    }   
+    else
+    {
+        line_pos[0] = 0.0f;
+    }
+    
+    /*-------------------交差点検出-------------------*/
+    if((mcp3208.adres[1] > adres_mid) && (mcp3208.adres[5] > adres_mid))
+    {
+        intersection = 1;
+    }
+    else if((mcp3208.adres[1] < adres_mid) && (mcp3208.adres[5] < adres_mid))
+    {
+        intersection = 0;
+    }
+    /*-----------------------------------------------*/
+    
+    /*---------------フィルタ----------------*/
+    
+    /*--------------------------------------*/
+    
+    /*-----------ジャイロ角速度取得-----------*/
+    omega = (float)l3gd20.z * 0.07f;
+    theta += omega * ts;
+    /*--------------------------------------*/
+    
+    /*------------------制御則-----------------*/
+    if(current_pid_ptr == &trace_pid)   //ライントレース
+    {
+        //vm_R = PD_Control(current_pid_ptr, omega, 10.0f * line_pos[0]);
+        vm_R = PD_Control(current_pid_ptr, -line_pos[0], 0.0f);
+        vm_L = -vm_R;
+        
+        vm_L += v_tr;
+        vm_R += v_tr;
+    }
+    else if(current_pid_ptr == &gyro_pid)
+    {
+        vm_R = PD_Control(current_pid_ptr, omega, omega_d);
+        vm_L = -vm_R;
+        
+        vm_L += v_tr;
+        vm_R += v_tr;
+    }
+    else
+    {
+        vm_L = 0.0f;
+        vm_R = 0.0f;
+    }
+    
+    if(current_pid_ptr == &manual_control)
+    {
+        //手動でモータ電圧を管理している場合
+    }
+    else
+    {
+        Put_Vm_L(vm_L); //モータへ電圧印加
+        Put_Vm_R(vm_R);
+    }
+    
+    /*----------------------------------------*/
+}
+
+void Flip3_Callback()       
+{
+    
+    
+    
+}
+
+void PinIntrrpt1_Callback()
+{
+    
+}
+
+void PinIntrrpt2_Callback()
+{
+    
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri May 26 03:51:19 2017 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/e1686b8d5b90
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mylib/devices/inc/GP2Y0E03.h	Fri May 26 03:51:19 2017 +0000
@@ -0,0 +1,10 @@
+#ifndef __GP2Y0E03__
+#define __GP2Y0E03__
+
+#include "adc.h"
+
+
+float Get_Distance(float );
+
+
+#endif  /* GP2Y0E03_H */
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mylib/devices/inc/L3GD20.h	Fri May 26 03:51:19 2017 +0000
@@ -0,0 +1,43 @@
+#ifndef __l3gd20_H
+#define __l3gd20_H
+
+#include "spi.h"
+//#include "mxconstants.h"
+#include <stdio.h>
+
+
+#define L3GD20_RW 0x80
+
+/*-------------------------------*/
+#define WHO_AM_I    0x0F
+#define CTRL_REG1   0x20
+#define CTRL_REG2   0x21
+#define CTRL_REG3   0x22
+#define CTRL_REG4   0x23
+
+#define OUT_TEMP    0x26
+
+#define OUT_X_L     0x28
+#define OUT_X_H     0x29
+#define OUT_Y_L     0x2A
+#define OUT_Y_H     0x2B
+#define OUT_Z_L     0x2C
+#define OUT_Z_H     0x2D
+/*-------------------------------*/
+
+typedef struct{
+    
+    int32_t x;
+    int32_t y;
+    int32_t z;
+    
+    int32_t temp;
+    
+    } gyro_t;
+    
+extern gyro_t l3gd20;
+
+void L3GD20_Init();
+void L3GD20_Read();
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mylib/devices/inc/MCP3208.h	Fri May 26 03:51:19 2017 +0000
@@ -0,0 +1,21 @@
+#ifndef __mcp3208_H
+#define __mcp3208_H
+
+#include "spi.h"
+
+/*These values include the start bit*/
+#define MCP3208_MODE_DIFF   0x04
+#define MCP3208_MODE_SINGLE 0x06
+
+typedef struct{
+    
+    int32_t adres[8];
+    
+    } adc_t;
+    
+extern adc_t mcp3208;
+
+void MCP3208_Init();
+void MCP3208_Read(uint8_t);
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mylib/devices/src/GP2Y0E03.cpp	Fri May 26 03:51:19 2017 +0000
@@ -0,0 +1,21 @@
+#include "GP2Y0E03.h"
+
+const float d_max = 0.50f;
+const float d_min = 0.04f;
+const float v_max = 2.20f;
+const float v_min = 0.55f;
+
+float Get_Distance(float vin)
+{
+    //return val: distance [m]
+    
+    float dist;
+    
+    dist = 0.6576f - vin * (d_max-d_min) / (v_max-v_min);   
+    if (dist> d_max)   //測定限界
+    {
+        return 0.0f;
+    }
+    
+    return dist;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mylib/devices/src/L3GD20.cpp	Fri May 26 03:51:19 2017 +0000
@@ -0,0 +1,69 @@
+#include "L3GD20.h"
+
+#define CS_GPIOx    GPIOA
+#define CS_GPIO_Pin GPIO_PIN_3
+
+gyro_t l3gd20;
+
+/*----------------SPI module-----------------*/
+static SPI *spi_ptr = &spi1;			//use SPI1
+static DigitalOut *cs_ptr = &spi1_cs1;	//CS pin
+/*-------------------------------------------*/
+
+void L3GD20_Init()
+{
+	*cs_ptr = 1;	//CS high
+	HAL_Delay(10);
+	
+	/*-----configuration-----*/
+	*cs_ptr = 0;	//CS low
+	(*spi_ptr).write(CTRL_REG1);
+	(*spi_ptr).write(0xCF);
+	*cs_ptr = 1;	//CS high
+	HAL_Delay(50);
+	
+	*cs_ptr = 0;	//CS low
+	(*spi_ptr).write(CTRL_REG4);
+	(*spi_ptr).write(0x30);
+	*cs_ptr = 1;	//CS high
+	/*-----------------------*/
+
+#if 0
+			spi_tx[0] = WHO_AM_I | L3GD20_RW;
+			spi_tx[1] = 0x00;
+			HAL_GPIO_WritePin(CS_GPIOx, CS_GPIO_Pin, GPIO_PIN_RESET);
+			HAL_SPI_TransmitReceive(&hspi1, spi_tx, spi_rx, 2, 10);
+			HAL_GPIO_WritePin(CS_GPIOx, CS_GPIO_Pin, GPIO_PIN_SET);
+
+			//printf("who am i = 0x%x\n", spi_rx[1]);
+			//while(spi_rx[1] == 0xFF);
+#endif
+	
+}
+
+void L3GD20_Read()
+{
+    uint32_t i;
+    
+    *cs_ptr = 0;	//CS low
+    //"0x40" MUST be added to increment the resister address
+    (*spi_ptr).write(OUT_X_L | L3GD20_RW | 0x40);
+    
+	for(i = 0; i < 6; i ++)
+	{
+		//spi_rx[0] is an invalid byte
+		//so this process starts with spi_rx[1]
+		spi_rx[i+1] = (*spi_ptr).write(0x00);	//dummy data
+	}
+	
+	*cs_ptr = 1;	//CS high
+	
+	l3gd20.x = ((int32_t)spi_rx[1]+((int32_t)spi_rx[2]<<8));
+	l3gd20.y = ((int32_t)spi_rx[3]+((int32_t)spi_rx[4]<<8));
+	l3gd20.z = ((int32_t)spi_rx[5]+((int32_t)spi_rx[6]<<8));
+	
+	if(l3gd20.z > 32768)
+	{
+		l3gd20.z = l3gd20.z - 65535 + 6;
+	}
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mylib/devices/src/MCP3208.cpp	Fri May 26 03:51:19 2017 +0000
@@ -0,0 +1,27 @@
+#include "MCP3208.h"
+
+#define CS_GPIOx GPIOA
+#define CS_GPIO_Pin GPIO_PIN_4
+
+adc_t mcp3208;
+
+/*----------------SPI module-----------------*/
+static SPI *spi_ptr = &spi1;			//use SPI1
+static DigitalOut *cs_ptr = &spi1_cs2;	//CS pin
+/*-------------------------------------------*/
+
+void MCP3208_Init()
+{
+	*cs_ptr = 1;	//CS high
+}
+
+void MCP3208_Read(uint8_t channel)
+{
+    *cs_ptr = 0;	//CS low
+    (*spi_ptr).write(MCP3208_MODE_SINGLE | ((channel & 0x04) >> 2));
+    spi_rx[1] = (*spi_ptr).write((channel & 0x03) << 6);
+    spi_rx[2] = (*spi_ptr).write(0x00);											//dummy data
+    *cs_ptr = 1;	//CS high
+		
+	mcp3208.adres[channel] = (((int32_t)spi_rx[1] << 8) + spi_rx[2]) & 0x0FFF;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mylib/inc/adc.h	Fri May 26 03:51:19 2017 +0000
@@ -0,0 +1,11 @@
+#include "mbed.h"
+
+#define ADC_VREF 3.3f       //ADC基準電圧
+
+/*----------Internal ADC------------*/
+extern AnalogIn ain_cup;    //測距センサ用ADCポート
+extern AnalogIn ain_lift;
+extern AnalogIn ain_shelf;
+/*----------------------------------*/
+
+float Ain_To_Vo(AnalogIn *);
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mylib/inc/gpio.h	Fri May 26 03:51:19 2017 +0000
@@ -0,0 +1,20 @@
+#ifndef __GPIO_H
+#define __GPIO_H
+
+#include "mbed.h"
+
+extern DigitalOut servo;
+
+/*---------Interrupt In----------*/
+extern InterruptIn intrrput_in1;
+extern InterruptIn intrrput_in2;
+/*-------------------------------*/
+
+void GPIO_Init();
+
+/*メインのソースコードにこの名称の関数を書く*/
+extern void PinIntrrpt1_Callback();
+extern void PinIntrrpt2_Callback();
+/*------------------------------------*/
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mylib/inc/spi.h	Fri May 26 03:51:19 2017 +0000
@@ -0,0 +1,14 @@
+#ifndef __SPI_H
+#define __SPI_H
+
+#include "mbed.h"
+
+extern uint8_t spi_tx[64];
+extern uint8_t spi_rx[64];
+
+extern SPI spi1;
+extern DigitalOut spi1_cs1;
+extern DigitalOut spi1_cs2;
+
+void SPI_Init();
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mylib/inc/tim.h	Fri May 26 03:51:19 2017 +0000
@@ -0,0 +1,26 @@
+#ifndef __TIM_H
+#define __TIM_H
+
+#include "mbed.h"
+
+/*-----繰り返しタイマ割り込み-----*/
+extern Ticker flipper1;
+extern Ticker flipper2;
+extern Ticker flipper3;
+/*----------------------------*/
+
+/*------------PWM-------------*/
+extern PwmOut pwm1;      //tim3 ch2
+extern PwmOut pwm2;     //tim1 ch3
+extern PwmOut pwm3;      //tim1 ch1
+/*----------------------------*/
+
+void TIM_Init();
+
+/*メインのソースコードにこの名称の関数を書く*/
+extern void Flip1_Callback();
+extern void Flip2_Callback();
+extern void Flip3_Callback();
+/*------------------------------------*/
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mylib/src/adc.cpp	Fri May 26 03:51:19 2017 +0000
@@ -0,0 +1,13 @@
+#include "adc.h"
+
+/*----------Internal ADC------------*/
+AnalogIn ain_cup(PA_0);    //測距センサ用ADCポート
+AnalogIn ain_lift(PA_1);
+AnalogIn ain_shelf(PA_4);
+/*----------------------------------*/
+
+float Ain_To_Vo(AnalogIn *ain)
+{
+    //アナログ値から電圧値を計算
+    return (ADC_VREF*(*ain));   
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mylib/src/gpio.cpp	Fri May 26 03:51:19 2017 +0000
@@ -0,0 +1,18 @@
+#include "gpio.h"
+
+/*----------GPIO output----------*/
+DigitalOut servo(PA_9);
+/*-------------------------------*/
+
+/*---------Interrupt In----------*/
+InterruptIn intrrpt_in1(PB_8);
+InterruptIn intrrpt_in2(PB_9);
+/*-------------------------------*/
+
+void GPIO_Init()
+{
+    /*-----------ピン変化割り込み-------------*/
+    intrrpt_in1.rise(&PinIntrrpt1_Callback);
+    intrrpt_in2.rise(&PinIntrrpt2_Callback);
+    /*--------------------------------------*/
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mylib/src/spi.cpp	Fri May 26 03:51:19 2017 +0000
@@ -0,0 +1,16 @@
+#include "spi.h"
+
+uint8_t spi_tx[64];
+uint8_t spi_rx[64];
+
+SPI spi1(PA_7, PA_6, PA_5); // mosi, miso, sclk
+/*-----CS pins for spi1-----*/
+DigitalOut spi1_cs1(PC_7);   
+DigitalOut spi1_cs2(PB_6);   
+/*--------------------------*/
+
+void SPI_Init()
+{
+    spi1.format(8, 3);   
+    spi1.frequency(1000000);
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mylib/src/tim.cpp	Fri May 26 03:51:19 2017 +0000
@@ -0,0 +1,29 @@
+#include "tim.h"
+
+/*-----繰り返しタイマ割り込み-----*/
+Ticker flipper1;
+Ticker flipper2;
+Ticker flipper3;
+/*----------------------------*/
+
+/*------------PWM-------------*/
+PwmOut pwm1(PB_5);      //tim3 ch2
+PwmOut pwm2(PA_10);     //tim1 ch3
+PwmOut pwm3(PA_8);      //tim1 ch1
+/*----------------------------*/
+
+
+void TIM_Init()
+{
+    /*----------Interval interrupt----------*/
+    flipper1.attach(&Flip1_Callback, 0.01);
+    flipper2.attach(&Flip2_Callback, 0.001);
+    //flipper3.attach(&Flip3_Callback, 0.001);
+    /*--------------------------------------*/
+    
+    /*--------pulse width modulation--------*/
+    pwm1.period(0.0001);    //10kHz
+    pwm2.period(0.0001);    //10kHz
+    pwm3.period(0.0001);    //10kHz
+    /*--------------------------------------*/
+}