Revision 0:e83b840a5f86, committed 2018-09-18

Comitter:

Komazawa_sun

Date:

Tue Sep 18 03:11:01 2018 +0000

Commit message:

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

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MDPIDSpeed/MDPIDSpeed.cpp	Tue Sep 18 03:11:01 2018 +0000
@@ -0,0 +1,70 @@
+/*
+ * MDPIDSpeed.cpp
+ *
+ *  Created on: 2018/08/31
+ *      Author: komazawataiyou
+ */
+
+#include "MDPIDSpeed.h"
+
+MD_PID_Speed::MD_PID_Speed(MD *md, QEI *encorder, double kp, double ki, double kd)
+: Timer_PID(kp, ki, kd), duty_(0.00), target_rpm_(0.00)
+{
+	this->md = md;
+	this->encorder = encorder;
+
+}
+
+MD_PID_Speed::~MD_PID_Speed() {
+}
+
+void MD_PID_Speed::drive(double rpm)
+{
+	double current_rpm = this->get_current_rpm();
+	target_rpm_ = rpm;
+
+	duty_ += Timer_PID::PID(current_rpm, rpm);
+
+	for(int i = 1; i < (signed)(sizeof(rpm_log_) / sizeof(double)); i++)
+		rpm_log_[i - 1] = rpm_log_[i];
+	rpm_log_[sizeof(rpm_log_) / sizeof(double) - 1] = current_rpm;
+
+	md->drive(duty_);
+}
+
+void MD_PID_Speed::brake(double rpm)
+{
+
+}
+
+void MD_PID_Speed::free()
+{
+
+}
+
+bool MD_PID_Speed::target_complete(float allowable_error)
+{
+	allowable_error = (fabs(allowable_error) < 1.0) ? fabs(allowable_error) : 1.00;
+	double rpm_mean = 0.00;
+	for(int i = 0; i < (signed)(sizeof(rpm_log_) / sizeof(rpm_log_[0])); i++)
+		rpm_mean += rpm_log_[i];
+	rpm_mean /= (sizeof(rpm_log_) / sizeof(double));
+
+	return (rpm_mean <= target_rpm_ + allowable_error && rpm_mean >= target_rpm_ - allowable_error);
+}
+
+double MD_PID_Speed::get_duty()
+{
+	return duty_;
+}
+
+double MD_PID_Speed::get_current_rpm()
+{
+	return encorder->speed() * 60;
+}
+
+void MD_PID_Speed::reset()
+{
+	duty_ = 0;
+	Timer_PID::reset();
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MDPIDSpeed/MDPIDSpeed.h	Tue Sep 18 03:11:01 2018 +0000
@@ -0,0 +1,40 @@
+/*
+ * MDPIDSpeed.h
+ *
+ *  Created on: 2018/08/31
+ *      Author: komazawataiyou
+ */
+
+#ifndef NUCLEO_FLIPING_ARM_LIB_MD_PID_SPEED_MDPIDSPEED_H_
+#define NUCLEO_FLIPING_ARM_LIB_MD_PID_SPEED_MDPIDSPEED_H_
+
+#include "mbed.h"
+#include "MD.h"
+#include "QEI.h"
+#include "Timer_PID.h"
+
+class MD_PID_Speed: public MD, public Timer_PID{
+public:
+	MD_PID_Speed(MD *md, QEI *encorder, double kp, double ki, double kd);
+	virtual ~MD_PID_Speed();
+
+	 virtual void drive(double rpm);
+	 virtual void brake(double rpm);
+	 virtual void free();
+
+	 double get_duty();
+	 double get_current_rpm();
+
+	 bool target_complete(float allowable_error);
+
+	 void reset();
+private:
+    MD *md;
+    QEI *encorder;
+
+    double target_rpm_;
+    double rpm_log_[4];
+    double duty_;
+};
+
+#endif /* NUCLEO_FLIPING_ARM_LIB_MD_PID_SPEED_MDPIDSPEED_H_ */

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/lib/Angle.h	Tue Sep 18 03:11:01 2018 +0000
@@ -0,0 +1,13 @@
+#ifndef ANGLE_H_
+#define ANGLE_H_
+
+class Angle
+{
+public:
+    virtual double read() = 0;
+    virtual void reset() = 0;
+    //virtual void set_angle_range(int angle_range) = 0;
+    //virtual void set_deg(int deg) = 0;
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/lib/LinearFunction.cpp	Tue Sep 18 03:11:01 2018 +0000
@@ -0,0 +1,50 @@
+#include "LinearFunction.h"
+
+LinearFunction::LinearFunction(unsigned int op_period_ms):
+    _op_period(((op_period_ms > 0) ?(op_period_ms < LF_MAX_TIME) ?op_period_ms :LF_MAX_TIME :1) / 1000.0)
+{
+    _ticker.attach(this, &LinearFunction::_on_operate, _op_period);    
+}
+
+void LinearFunction::set(const double val, const double intercept, const unsigned int ct_ms)
+{
+    
+    if(_range !=  val - intercept){
+        reset();
+        _range = val - intercept;
+    }
+    
+    _intercept = intercept;
+    
+    _timer.start();    
+    _ct_s = ((ct_ms > 0) ?(ct_ms > _op_period * LF_MAX_TIME) ?ct_ms :_op_period * LF_MAX_TIME :1) / 1000.0;    
+}
+
+void LinearFunction::reset()
+{
+    _ct_s = 0;
+    _range = 0;
+    _intercept = 0;
+    _current_val = 0;
+    _timer.reset();
+    _timer.stop();
+}
+
+double LinearFunction::get_val()
+{
+    return _current_val + _intercept;
+}
+
+void LinearFunction::_on_operate()
+{
+    
+    if(_ct_s <= _timer.read_ms() / 1000.0){
+        _current_val = _range;
+        _timer.stop();
+        if(_current_val != _range)
+            _timer.reset();
+    }else{
+        _current_val = (_range / _ct_s) * (_timer.read_ms() / 1000.0);
+    }
+    
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/lib/LinearFunction.h	Tue Sep 18 03:11:01 2018 +0000
@@ -0,0 +1,29 @@
+#ifndef LINER_FUNCTION_H_
+#define LINER_FUNCTION_H_
+
+#include "mbed.h"
+
+#define LF_MAX_TIME 1000
+
+class LinearFunction
+{
+public:
+    LinearFunction(unsigned int op_period_ms);
+    void set(const double val, const double intercept, const unsigned int ct_ms);
+    void reset();
+    double get_val();
+private:
+    double _op_period;
+    //double _val;
+    double _range;
+    double _intercept;
+    double _ct_s;
+    double _current_val;
+    
+    Ticker _ticker;
+    Timer _timer;
+    
+    void _on_operate();
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/lib/MD.h	Tue Sep 18 03:11:01 2018 +0000
@@ -0,0 +1,13 @@
+#ifndef MD_H_
+#define MD_H_
+
+class MD
+{
+public:
+    virtual ~MD(){}
+    virtual void drive(double) = 0;
+    virtual void brake(double) = 0;
+    virtual void free() = 0;
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/lib/PID_Control.cpp	Tue Sep 18 03:11:01 2018 +0000
@@ -0,0 +1,38 @@
+#include "PID_Control.h"
+
+PID_Control::PID_Control(double kp, double ki, double kd)
+{
+    set_PID(kp, ki, kd);
+    integral = 0.0;
+}
+
+double PID_Control::PID(double present, double target, double interval)
+{
+    diff[0] = diff[1];
+    diff[1] = target - present;
+    integral += ((diff[0] + diff[1]) / 2.0) * interval;
+    p = kp * diff[1];
+    i = ki * integral;
+    d = kd * (diff[1] - diff[0]);
+    control = p + i + d;
+    //printf("p[%f] i[%f] d[%f] ", p, i, d);
+    //printf("p:%.3f\t:%.3f\tc:%.3f\t:%.3f\r\n", present, target, control, interval);
+    //printf("t:%.3f\r\n", target);
+    return control;
+}
+
+void PID_Control::reset()
+{
+    diff[0] = 0;
+    diff[1] = 0.0;
+    integral = 0.0;
+    control = 0;
+}
+
+void PID_Control::set_PID(double kp, double ki, double kd)
+{
+    this->kp = kp;
+    this->ki = ki;
+    this->kd = kd;
+    reset();
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/lib/PID_Control.h	Tue Sep 18 03:11:01 2018 +0000
@@ -0,0 +1,24 @@
+#ifndef PID_CONTROL_H_
+#define PID_CONTROL_H_
+
+#include <mbed.h>
+
+class PID_Control
+{
+public:
+    PID_Control(double kp = 0, double ki = 0, double kd = 0);
+    
+    double PID(double present, double target, double interval);
+    void reset();
+    void set_PID(double kp, double ki, double kd);
+        
+private:
+    
+    double diff[2];
+    double integral;
+    double p, i, d;
+    double kp, ki, kd;
+    double control;
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/lib/Pin.h	Tue Sep 18 03:11:01 2018 +0000
@@ -0,0 +1,20 @@
+#ifndef PIN_H_
+#define PIN_H_
+
+#include "mbed.h"
+
+namespace pin{
+	const PinName sda 	= D2;
+	const PinName scl 	= D8;
+	const PinName ft_ts = D6;
+	const PinName led1 	= /*PB_15*/D13;
+	const PinName led2 	= PB_14;
+	const PinName led3 	= PB_13;
+	const PinName esv 	= PB_7;
+	const PinName pwm 	= D7;
+	const PinName dir 	= D3;
+	const PinName a_lyr = D4;
+	const PinName b_lyr = D5;
+}
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/lib/QEI.cpp	Tue Sep 18 03:11:01 2018 +0000
@@ -0,0 +1,126 @@
+#include "QEI.h"
+#include "mbed.h"
+
+QEI::QEI(PinName A, PinName B, int pulses, float t) :
+		A_(A), B_(B) {
+	count_ = 0;
+	over_count_ = 0;
+	oldcount_ = 0;
+	speed_ = 0;
+	oldspeed_ = 0;
+	pulses_ = pulses * 4;
+	revolutions_ = 0;
+	t_ = t;
+
+	x_.attach(this, &QEI::_sa, t_);
+
+	A_.rise(this, &QEI::_AR);
+	A_.fall(this, &QEI::_AF);
+	B_.rise(this, &QEI::_BR);
+	B_.fall(this, &QEI::_BF);
+}
+
+double QEI::read() {
+	return angle();
+}
+
+void QEI::reset() {
+	count_ = 0;
+	over_count_ = 0;
+	oldcount_ = 0;
+}
+
+void QEI::RevorutionCounter() {
+	if (count_ == pulses_) {
+		count_ = 0;
+		revolutions_++;
+	} else if (count_ < 0) {
+		count_ = pulses_ - 1;
+		revolutions_--;
+	}
+
+	if(over_count_ >= pulses_ * 20){
+		over_count_ = pulses_ * 20;
+	} else if (over_count_ <= -1.0 * pulses_ * 20) {
+		over_count_ = -1.0 * pulses_ * 20;
+	}
+
+}
+
+float QEI::over_angle(){
+	return ((float) over_count_ / (float) pulses_);
+}
+
+int QEI::over_count(){
+	return (over_count_);
+}
+
+int QEI::count() {
+	return (count_);
+}
+
+int QEI::revolution() {
+	return (revolutions_);
+}
+
+float QEI::angle() {
+	return ((float) count_ / (float) pulses_);
+}
+
+void QEI::_sa() {
+	oldspeed_ = speed_;
+	speed_ = ((float) ((revolutions_ * pulses_) + count_) - oldcount_) / t_
+			/ pulses_;
+	acceleration_ = (speed_ - oldspeed_) / t_;
+	oldcount_ = (revolutions_ * pulses_) + count_;
+}
+
+float QEI::speed() {
+	return (speed_);
+}
+
+float QEI::acceleration() {
+	return (acceleration_);
+}
+
+void QEI::_AR() {
+	if (B_.read()) {
+		count_--;
+		over_count_--;
+	} else if (!B_.read()) {
+		count_++;
+		over_count_++;
+	}
+	this->QEI::RevorutionCounter();
+}
+
+void QEI::_AF() {
+	if (B_.read()) {
+		count_++;
+		over_count_++;
+	} else if (!B_.read()) {
+		count_--;
+		over_count_--;
+	}
+	this->QEI::RevorutionCounter();
+}
+void QEI::_BR() {
+	if (A_.read()) {
+		count_++;
+		over_count_++;
+	} else if (!A_.read()) {
+		count_--;
+		over_count_--;
+	}
+	this->QEI::RevorutionCounter();
+}
+void QEI::_BF() {
+	if (A_.read()) {
+		count_--;
+		over_count_--;
+	} else if (!A_.read()) {
+		count_++;
+		over_count_++;
+	}
+	this->QEI::RevorutionCounter();
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/lib/QEI.h	Tue Sep 18 03:11:01 2018 +0000
@@ -0,0 +1,40 @@
+#ifndef MBED_QEI
+#define MBED_QEI
+#include "mbed.h"
+#include "Angle.h"
+
+class QEI : public Angle{
+public:
+    QEI(PinName A, PinName B, int pulses, float t);
+    virtual double read();
+    virtual void reset();
+    void RevorutionCounter();
+    float over_angle();
+    int over_count();
+    int count();
+    int revolution();
+    float angle();
+    float acceleration();
+    float speed();
+protected:
+    int pulses_;   
+private:
+    Ticker x_;
+    InterruptIn A_, B_;
+    int count_;
+    int over_count_;
+    int32_t oldcount_;
+    int revolutions_;
+    float speed_;
+    float oldspeed_;
+    float acceleration_;
+    float t_;
+    
+    void _AR();
+    void _AF();
+    void _BR();
+    void _BF();
+    void _sa();
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/lib/StraightMD.cpp	Tue Sep 18 03:11:01 2018 +0000
@@ -0,0 +1,30 @@
+#include"mbed.h"
+#include"StraightMD.h"
+StraightMD::StraightMD(PinName pwm,PinName dere):Pwm(pwm),Dere(dere)
+{
+    Pwm.period(0.0001);
+}
+
+
+void StraightMD::drive(double duty)
+{
+    if(duty > 0)
+    {
+        Dere = 0;
+        Pwm = abs(duty);
+    }
+    else
+    {
+        Dere = 1;
+        Pwm = abs(duty);
+    }
+}
+
+void StraightMD::brake(double duty){
+}
+
+
+
+void StraightMD::free()
+{
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/lib/StraightMD.h	Tue Sep 18 03:11:01 2018 +0000
@@ -0,0 +1,19 @@
+#ifndef _MD_H_
+#define _MD_H_
+
+#include <MD.h>
+
+class StraightMD : public MD
+{
+public:
+    StraightMD(PinName pwm,PinName dere);
+    virtual void drive(double);
+    virtual void brake(double);
+    virtual void free();
+    //void rotate(double duty);
+private:
+    PwmOut Pwm;
+    DigitalOut Dere;
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/lib/Timer_PID.cpp	Tue Sep 18 03:11:01 2018 +0000
@@ -0,0 +1,27 @@
+#include "Timer_PID.h"
+
+Timer_PID::Timer_PID(double kp, double ki, double kd) : PID_Control(kp, ki, kd)
+{
+    reset();
+}
+
+double Timer_PID::PID(double present, double target)
+{
+    interval = read_interval();
+    return PID_Control::PID(present, target, interval);
+}
+
+double Timer_PID::read_interval()
+{
+    double time;
+    time = timer.read();
+    timer.reset();
+    return time;
+}
+
+void Timer_PID::reset()
+{
+    PID_Control::reset();
+    timer.reset();
+    timer.start();
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/lib/Timer_PID.h	Tue Sep 18 03:11:01 2018 +0000
@@ -0,0 +1,19 @@
+#ifndef TIMER_PID_H_
+#define TIMER_PID_H_
+
+#include <mbed.h>
+#include "PID_Control.h"
+
+class Timer_PID : public PID_Control
+{
+public:
+    Timer_PID(double kp = 0, double ki = 0, double kd = 0);
+    double PID(double present, double target);
+    double read_interval();
+    void reset();
+protected:
+    Timer timer;
+    double interval;
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Sep 18 03:11:01 2018 +0000
@@ -0,0 +1,342 @@
+#include <string.h>
+
+#include "mbed.h"
+#include "Pin.h"
+#include "LinearFunction.h"
+#include "QEI.h"
+#include "MD.h"
+#include "StraightMD.h"
+#include "Timer_PID.h"
+#include "MDPIDSpeed.h"
+
+#define MY_ADDRESS 0xd0
+
+#define M_PI 3.14159265359
+#define DEBUG_ENABLE 0
+#if DEBUG_ENABLE == 1
+#define DEBUG if(DEBUG_ENABLE)
+#define NDEBUG if(!DEBUG_ENABLE)
+#else
+#define DEBUG if(DEBUG_ENABLE)
+#define NDEBUG if(!DEBUG_ENABLE)
+#endif
+#define ROTE_LIMIT ((360.0 * 10) / 360.0)
+#define QEI_MAX_PULSES 512		//エンコーダパルス数(1層分)
+//#define CONTROL_INTERVAL 25		//制御周期
+
+#define DIM_DEG (double)360.00
+
+const double begin_angle = 5.0 / 360.00;		//初期角
+double shoot_angle = (325.0 + 9) / 360.00;		//発射角　//成功vパラメータ
+double shoot_open_angle = (295.0 + 10 + 10) / 360.00;	//成功vパラメータ
+
+double shoot_speed = -210.0; 				//発射速度[RPM]
+const double speed_limit = 1500;
+
+const double offset_allowable_error = begin_angle * (13.0 / 100.0);	//初期角許容誤差率
+const double shoot_allowable_error = shoot_speed * (1.5 / 100.0);	//発射速度許容誤差率
+const double shoot_ang_allowable_error = 5.0 / 360.0;
+
+const unsigned int topspeed_time_ms = 300;		//射出速度に達するまでの時間
+
+const unsigned int convergence_time_ms = 2000;	//初期収束時間
+
+const double offset_pwr_limit = 0.6;			//初期角収束時の出力制限
+const double offset_pwr_gain = 0.10;			//オフセット時のPWM出力
+
+const double const_moment = 0.027;				//角度制御で考慮するモーメントの大きさの係数
+
+
+const bool encorder_rev = true;
+
+I2CSlave slave(pin::sda, pin::scl);
+Ticker I2C_event_trigger;
+Ticker read_angle_timer;
+char I2C_cmd_shoot[4] = "SHT";
+char I2C_cmd_grip[4]  = "GRP";
+char I2C_cmd_reset[4] = "RST";
+const int I2C_buffer_size = 16;
+bool I2C_shoot_start = false;
+
+LinearFunction line(1);
+
+InterruptIn on_zero_point(pin::ft_ts);
+
+//DigitalIn button(A3);
+DigitalIn button(PC_13);
+
+DigitalOut dbg_led[3] =
+{ pin::led1, pin::led2, pin::led3 };
+DigitalOut ESV(pin::esv);
+
+PwmOut pwm_out(pin::pwm);
+DigitalOut dir_out(pin::dir);
+
+MD *output;
+QEI *rote;
+
+//------制御動作部分-----------------------
+typedef struct PIDConstract{
+    const double kp;
+    const double ki;
+    const double kd;
+}PID_constract;
+
+//PID_constract PID_const[2] = {{5.6, 2.5, 0.05},{0.00025, 0.00001, 0.000007}};
+PID_constract PID_const[2] = {{5.6, 2.5, 0.05},{0.00020, 0.00001, 0}};
+
+Timer_PID Position_PID(PID_const[0].kp, PID_const[0].ki, PID_const[0].kd);
+
+bool offset_position(bool dir_enable_CW);
+bool set_position(double angle, int time_ms, double allowable_error,
+		double pwr_limit);
+//------------------------------------------
+
+//------腕動作のリセット及び角度測定部分------------
+bool on_offset = false;
+bool rote_reset_enable = false;
+void encorder_angle_reset();
+//-----------------------------------------
+
+//------動作モードトグル部分---------------------
+bool dump_button(bool input, bool reset);
+bool reset = 0;
+//------------------------------------------
+
+void move_ESV(DigitalOut *output, bool close);
+
+double read_angle(bool enable_over_angle, bool dir_rev);
+void on_I2C_event();
+
+void on_read_angle();
+double real_time_angle;
+
+int main()
+{
+	button.mode(PullUp);
+	//slave.address(MY_ADDRESS);
+	//I2C_event_trigger.attach(&on_I2C_event, 100);
+    output = new StraightMD(pin::pwm, pin::dir);
+    rote = new QEI(pin::a_lyr, pin::b_lyr, QEI_MAX_PULSES, 0.005);
+
+    MD_PID_Speed Speed_PID(output, rote, PID_const[1].kp, PID_const[1].ki, PID_const[1].kd);
+
+    on_zero_point.rise(&encorder_angle_reset);
+    
+    on_offset = false;
+    while(!offset_position(false)){
+        DEBUG printf("move to offset(init)\tANGLE : %lf\r\n", read_angle(true, encorder_rev) * DIM_DEG);
+    }
+    DEBUG printf("Initial Ready\r\n");
+    
+    while(1){
+        bool shot_button = dump_button(!button.read() | I2C_shoot_start, reset);;
+        //on_offset = false;
+        
+        if(shot_button){//発射管制モード
+        	dbg_led[2].write(false);
+        	move_ESV(&ESV, true);
+        	wait_ms(500);
+
+            line.reset();
+            Position_PID.reset();
+
+			
+			read_angle_timer.attach_us(&on_read_angle, 100);
+            do{
+            	
+            	//line.set(shoot_speed, 0, topspeed_time_ms);
+            	//line.set(-0.70 + 0.079, 0, topspeed_time_ms); //成功vパラメータ
+            	line.set(-0.70 + 0.0795, 0, topspeed_time_ms); 
+            	//Speed_PID.drive(line.get_val());
+            	//Speed_PID.drive(shoot_speed);
+            	output->drive(line.get_val());
+            	if(
+            		/*read_angle(true, encorder_rev) >= 1.0
+            		&& read_angle(false, encorder_rev) <= shoot_open_angle
+					&& read_angle(false, encorder_rev) >= (shoot_open_angle - shoot_ang_allowable_error))*/
+					read_angle(true, encorder_rev) >= 1.0
+					&& real_time_angle <= shoot_open_angle
+					&& real_time_angle >= (shoot_open_angle - shoot_ang_allowable_error)){
+            		move_ESV(&ESV, false);
+
+            		while(/*read_angle(false, encorder_rev)*/real_time_angle <= shoot_angle){
+            			//Speed_PID.drive(line.get_val());
+            			output->drive(line.get_val());
+            		}
+            		//wait_ms(250);
+            		output->drive(0.00);
+            		DEBUG printf("stop!\r\n");
+            		//Speed_PID.reset();
+            		 line.reset();
+            		wait_ms(500);
+            		break;
+            	}
+            	//DEBUG printf("SpeedPID TAR : %.1lf_%.1lf RPM : %lf\tPWM : %lf\tF_ANG : %.1lf\tANG : %.3lf\r\n",
+            	//		shoot_speed  + shoot_allowable_error, shoot_speed - shoot_allowable_error, Speed_PID.get_current_rpm(), Speed_PID.get_duty(), read_angle(true, encorder_rev), read_angle(false, encorder_rev));
+            	DEBUG printf("%lf \r\n", Speed_PID.get_current_rpm()/*real_time_angle*/);
+            	//NDEBUG wait_ms(5);
+            	//Speed_PID.read_interval();
+            }while(fabs(read_angle(true, encorder_rev)) < ROTE_LIMIT);
+			read_angle_timer.detach();
+			
+            //Speed_PID.reset();
+            line.reset();
+            line.set(0.0, Speed_PID.get_current_rpm(), 1000);
+            do{
+            	Speed_PID.drive(line.get_val());
+            	DEBUG printf("breaking : %lf\r\n", line.get_val());
+            	NDEBUG wait_ms(50);
+            //wait_ms(500);
+            }while(line.get_val() != 0);
+
+            wait_ms(900);
+
+            Speed_PID.reset();
+            I2C_shoot_start = false;
+            on_offset = false;
+            while(!offset_position(false)){
+            	DEBUG printf("move to offset\tANGLE : %lf\r\n", read_angle(true, encorder_rev) * DIM_DEG);
+            }
+
+            move_ESV(&ESV, false);
+            line.reset();
+            Position_PID.reset();
+            reset = 1;
+
+        }else{//姿勢管制モード
+        	if(!set_position(begin_angle, convergence_time_ms, offset_allowable_error, offset_pwr_limit)){
+        		reset = 0;
+        		dbg_led[2].write(true);
+        	}else{
+        		dbg_led[2].write(false);
+        	}
+            Position_PID.read_interval();
+        }
+    }
+}
+
+bool dump_button(bool input, bool reset)
+{
+	static bool pushed_flg = false;
+
+	if (input && !pushed_flg)
+	{
+		pushed_flg = true;
+	}
+
+	if (reset)
+	{
+		pushed_flg = false;
+	}
+
+	return pushed_flg;
+}
+
+bool offset_position(bool dir_enable_CW)
+{
+    double pwr = (dir_enable_CW) ? offset_pwr_gain * -2.0 : offset_pwr_gain * 2.0;
+    
+    if(fabs(read_angle(true, encorder_rev)) <= 19.0){
+    	pwr = (dir_enable_CW) ? offset_pwr_gain * -1 : offset_pwr_gain;
+    	rote_reset_enable = true;
+    }
+
+    if(!on_offset){
+        output->drive(pwr);
+        wait_ms(10);
+        return false;
+    }else{
+    	rote_reset_enable = false;
+        output->drive(0.00);
+        wait_ms(500);
+        return true;
+    }   
+}
+
+bool set_position(double angle, int time_ms, double allowable_error,
+		double power_limit)
+{
+    static double standard_angle = read_angle(true, encorder_rev);
+
+    line.set(angle, standard_angle, time_ms);
+    double duty = -1 * Position_PID.PID(read_angle(true, encorder_rev), line.get_val()) - cos(2 * M_PI * read_angle(true, encorder_rev)) * const_moment;
+    if(duty >= power_limit)
+    	duty = power_limit;
+
+    output->drive(duty);
+    //DEBUG printf("position PID TAR : %lf-%lf ANGLE : %lf PWM : %lf\r\n", (line.get_val() + allowable_error) * DIM_DEG, (line.get_val() - allowable_error) * DIM_DEG, read_angle(true, encorder_rev) * DIM_DEG, duty);
+    //DEBUG printf("set_position\r\n");
+    wait_ms(2);
+
+    if(((angle  + allowable_error) < read_angle(true, encorder_rev) || (angle  - allowable_error) > read_angle(true, encorder_rev))){
+        return true;
+    }else{
+    	return false;
+    }
+}
+
+void encorder_angle_reset()
+{
+	if (rote_reset_enable)
+	{
+		printf("reset");
+		rote->reset();
+		on_offset = true;
+	}
+}
+
+void move_ESV(DigitalOut *output, bool close)
+{
+	dbg_led[0].write(close);
+	output->write(close);
+}
+
+double read_angle(bool enable_over_angle, bool dir_rev)
+{
+	double angle = 0;
+	if(enable_over_angle == true)
+		angle = (dir_rev) ? -1 * rote->over_angle() : 1 * rote->over_angle();
+	else
+		angle = (dir_rev) ? 1.00 - rote->angle() : rote->angle();
+	return angle;
+}
+
+void on_read_angle(){
+	real_time_angle = read_angle(false, encorder_rev);
+}
+
+//SHT 角度[deg] 速度[rpm] 	:発射コマンド
+//SHT 000.0 0000.0 		|16文字
+//GRP 					:掴みコマンド（発射用フラグ）
+//RST					:リセットコマンド
+
+void on_I2C_event()
+{
+	char buffer[I2C_buffer_size] = {};
+	switch (slave.receive())
+	{
+	case I2CSlave::ReadAddressed:
+		sprintf(buffer, "%3.1lf %4.1lf %d", shoot_angle, shoot_speed, reset);
+		slave.write(buffer, I2C_buffer_size);
+		break;
+	case I2CSlave::WriteAddressed:
+		slave.read(buffer, I2C_buffer_size);
+		char cmd[4] = {buffer[0], buffer[1], buffer[2], '\0'};
+
+		if(strcmp(cmd, I2C_cmd_shoot) == 0 && ESV.read()){
+			char option_angle[5] = {buffer[4], buffer[5], buffer[6], buffer[7], buffer[8]};
+			char option_speed[6] = {buffer[10], buffer[11], buffer[12], buffer[13], buffer[14], buffer[15]};
+
+			shoot_angle = (360 >= atof(option_angle) && 0 <= atof(option_angle)) ? atof(option_angle) / 360.0 : shoot_angle;
+			shoot_speed = (speed_limit >= atof(option_speed) && 0 <= atof(option_speed)) ? atof(option_speed) : shoot_speed;
+
+			I2C_shoot_start = true;
+		}else if(strcmp(cmd, I2C_cmd_grip) == 0 && !I2C_shoot_start)
+			move_ESV(&ESV, true);
+		else if(strcmp(cmd, I2C_cmd_reset) == 0)
+			NVIC_SystemReset();
+		break;
+	};
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Sep 18 03:11:01 2018 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/e95d10626187
\ No newline at end of file