File content as of revision 0:e83b840a5f86:

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