<repo_name>wadadones/ET<file_sep>/1108_R/Operator/Operator.cpp
// Operator.cpp

#include "Operator.hpp"

//�J�E���g�A�b�v����
#define COUNT_TIME 4
//�^�X�N���~����
#define SLEEP_TIME 3

//�R���X�g���N�^
Operator::Operator(EV3* ev3, Balancer* balancer, Schedule* schedule)
{
	this->ev3 = ev3;
	this->balancer = balancer;
	this->schedule = schedule;
}

void Operator::run()
{
	//���s���t�F�[�Y�̃|�C���^
	Phase* currentPhase;
	//�e�t�F�[�Y�̌o�ߎ���
	int time;
	//�Z���T�����̓���
	int32_t motor_ang_l;
	int32_t motor_ang_r;
	int color;
	int gyro;
	int sonic;
	int volt;
	//���[�^�ւ̏o��
	signed char pwm_L;
	signed char pwm_R;
	signed int ang_T;

	//EV3�̐ݒ�
	ev3->setUpEV3();
	//�X�^�[�g�w���҂�
	ev3->waitForStart();
	//�Z���T�̃��Z�b�g
	ev3->resetSensorValue();
	//�|�����s���C�u�����̏�����
	balancer->init();

	while(schedule->isEmpty()){
		//���̃t�F�[�Y�������o��
		currentPhase = schedule->popPhase();
		//�t�F�[�Y�̏�������
		currentPhase->initialize();
		//�^�C�}�[�̃��Z�b�g
		time = 0;
		//�X�^�[�g�ʒm
		ev3->playTone();

		do{
			//�Z���T�������͂��Ⴄ
			ev3->getSensorValue(&motor_ang_l, &motor_ang_r, &color, &gyro, &sonic, &volt);
			//���͒l�����o�͒l���t�F�[�Y���Ƃ̕��@�ŎZ�o
			currentPhase->calculate(time, motor_ang_l, motor_ang_r, color, gyro, sonic, volt, &pwm_L, &pwm_R, &ang_T);
			//���[�^�ɏo�͂��n��
			ev3->setMotorValue(pwm_L, pwm_R, ang_T);
			//���₷��
			ev3->sleep(SLEEP_TIME);
			//�^�C�}�[�̃C���N�������g
			time += COUNT_TIME;
			//�����̃{�^���ŏI��
			if(ev3->buttonIsPressed()) break;
		}while(!currentPhase->isFinished(time, motor_ang_l, motor_ang_r, color, gyro, sonic, volt, pwm_L, pwm_R, ang_T));

		//�����̃{�^����ry
		if(ev3->buttonIsPressed()) break;
	}

	//�I���ʒm
	ev3->playTone();
	//EV3�̏I������
	ev3->shutDownEV3();

	return;
}
<file_sep>/1108_L/EV3/EV3.cpp
// EV3.cpp

#include "EV3.hpp"

/*?Z???T?[?????????`?????*/
static const sensor_port_t
	touch_sensor    = EV3_PORT_1,
	sonar_sensor    = EV3_PORT_2,
	color_sensor    = EV3_PORT_3,
	gyro_sensor     = EV3_PORT_4;

/*???[?^?????????`?????*/
static const motor_port_t
	left_motor      = EV3_PORT_C,
	right_motor     = EV3_PORT_B,
	tail_motor      = EV3_PORT_A;

/* LCD?t?H???g?T?C?Y */
#define CALIB_FONT (EV3_FONT_SMALL)
#define CALIB_FONT_WIDTH (6/*TODO: magic number*/)
#define CALIB_FONT_HEIGHT (8/*TODO: magic number*/)

/* ???S???~????p?x[?x] */
#define TAIL_ANGLE_STAND_UP  98
/* ???S???~?p???[?^?????????W?? */
#define P_GAIN             2.5F
/* ???S???~?p???[?^????PWM???????l */
#define PWM_ABS_MAX          60

/* ?g?[??????? */
#define TONE_F              500
/* ?g?[??????? */
#define TONE_T             1000

/* Bluetooth?t?@?C???n???h?? */
static FILE     *bt = NULL;
/* Bluetooth?R?}???h 1:?????[?g?X?^?[?g */
static int      bt_cmd = 0;

void EV3::setUpEV3()
{
	/* ?Z???T?[????|?[?g???? */
	ev3_sensor_config(sonar_sensor, ULTRASONIC_SENSOR);
	ev3_sensor_config(color_sensor, COLOR_SENSOR);
	ev3_color_sensor_get_reflect(color_sensor); /* ????????[?h */
	ev3_sensor_config(touch_sensor, TOUCH_SENSOR);
	ev3_sensor_config(gyro_sensor, GYRO_SENSOR);

	/* ???[?^?[?o??|?[?g???? */
	ev3_motor_config(left_motor, LARGE_MOTOR);
	ev3_motor_config(right_motor, LARGE_MOTOR);
	ev3_motor_config(tail_motor, LARGE_MOTOR);
	ev3_motor_set_power(tail_motor, -30);
	tslp_tsk(1000);
	ev3_motor_reset_counts(tail_motor);

	/* LCD????\?? */
	ev3_lcd_fill_rect(0, 0, EV3_LCD_WIDTH, EV3_LCD_HEIGHT, EV3_LCD_WHITE);
	int mv = ev3_battery_voltage_mV();
	if(mv >= 9000){
		ev3_lcd_draw_string("over 9000", 0, CALIB_FONT_HEIGHT*1);
	}else if(mv >= 8900){
		ev3_lcd_draw_string("over 8900", 0, CALIB_FONT_HEIGHT*1);
	}else if(mv >= 8800){
		ev3_lcd_draw_string("over 8800", 0, CALIB_FONT_HEIGHT*1);
	}else if(mv >= 8700){
		ev3_lcd_draw_string("over 8700", 0, CALIB_FONT_HEIGHT*1);
	}else if(mv >= 8600){
		ev3_lcd_draw_string("over 8600", 0, CALIB_FONT_HEIGHT*1);
	}else if(mv >= 8500){
		ev3_lcd_draw_string("over 8500", 0, CALIB_FONT_HEIGHT*1);
	}else if(mv >= 8400){
		ev3_lcd_draw_string("over 8400", 0, CALIB_FONT_HEIGHT*1);
	}else if(mv >= 8300){
		ev3_lcd_draw_string("over 8300", 0, CALIB_FONT_HEIGHT*1);
	}else if(mv >= 8200){
		ev3_lcd_draw_string("over 8200", 0, CALIB_FONT_HEIGHT*1);
	}else if(mv >= 8100){
		ev3_lcd_draw_string("over 8100", 0, CALIB_FONT_HEIGHT*1);
	}else if(mv >= 8000){
		ev3_lcd_draw_string("over 8000", 0, CALIB_FONT_HEIGHT*1);
	}else if(mv >= 7900){
		ev3_lcd_draw_string("over 7900", 0, CALIB_FONT_HEIGHT*1);
	}else if(mv >= 7800){
		ev3_lcd_draw_string("over 7800", 0, CALIB_FONT_HEIGHT*1);
	}else if(mv >= 7700){
		ev3_lcd_draw_string("over 7700", 0, CALIB_FONT_HEIGHT*1);
	}else if(mv >= 7600){
		ev3_lcd_draw_string("over 7600", 0, CALIB_FONT_HEIGHT*1);
	}else if(mv >= 7500){
		ev3_lcd_draw_string("over 7500", 0, CALIB_FONT_HEIGHT*1);
	}else if(mv >= 7400){
		ev3_lcd_draw_string("over 7400", 0, CALIB_FONT_HEIGHT*1);
	}else if(mv >= 7300){
		ev3_lcd_draw_string("over 7300", 0, CALIB_FONT_HEIGHT*1);
	}else if(mv >= 7200){
		ev3_lcd_draw_string("over 7200", 0, CALIB_FONT_HEIGHT*1);
	}else if(mv >= 7100){
		ev3_lcd_draw_string("over 7100", 0, CALIB_FONT_HEIGHT*1);
	}else if(mv >= 7000){
		ev3_lcd_draw_string("over 7000", 0, CALIB_FONT_HEIGHT*1);
	}else if(mv >= 6900){
		ev3_lcd_draw_string("over 6900", 0, CALIB_FONT_HEIGHT*1);
	}else if(mv >= 6800){
		ev3_lcd_draw_string("over 6800", 0, CALIB_FONT_HEIGHT*1);
	}else if(mv >= 6700){
		ev3_lcd_draw_string("over 6700", 0, CALIB_FONT_HEIGHT*1);
	}else if(mv >= 6600){
		ev3_lcd_draw_string("over 6600", 0, CALIB_FONT_HEIGHT*1);
	}else if(mv >= 6500){
		ev3_lcd_draw_string("over 6500", 0, CALIB_FONT_HEIGHT*1);
	}else if(mv >= 6400){
		ev3_lcd_draw_string("over 6400", 0, CALIB_FONT_HEIGHT*1);
	}else if(mv >= 6300){
		ev3_lcd_draw_string("over 6300", 0, CALIB_FONT_HEIGHT*1);
	}else if(mv >= 6200){
		ev3_lcd_draw_string("over 6200", 0, CALIB_FONT_HEIGHT*1);
	}else if(mv >= 6100){
		ev3_lcd_draw_string("over 6100", 0, CALIB_FONT_HEIGHT*1);
	}else if(mv >= 6000){
		ev3_lcd_draw_string("over 6000", 0, CALIB_FONT_HEIGHT*1);
	} else {
		ev3_lcd_draw_string("under 6000", 0, CALIB_FONT_HEIGHT*1);
	}

	/* Open Bluetooth file */
	bt = ev3_serial_open_file(EV3_SERIAL_BT);
	assert(bt != NULL);

	/* Bluetooth??M?^?X?N??N?? */
	act_tsk(BT_TASK);

	/* ????????????m */
	ev3_led_set_color(LED_ORANGE);
}

void EV3::waitForStart()
{
	/* ?X?^?[?g??@ */
	int i = TAIL_ANGLE_STAND_UP;
	//int flag = 0;
	while(1){
		ev3_tail_set_angle(i); /* ???S???~?p?p?x????? */

		if(ev3_button_is_pressed(UP_BUTTON)){
			i--;
			while(1){
				if(!ev3_button_is_pressed(UP_BUTTON)) break;
			}
		}
		if(ev3_button_is_pressed(DOWN_BUTTON)){
			i++;
			while(1){
				if(!ev3_button_is_pressed(DOWN_BUTTON)) break;
			}
		}

		if (bt_cmd == 1){
			break; /* ?????[?g?X?^?[?g */
		}

		if (ev3_touch_sensor_is_pressed(touch_sensor) == 1){
			break; /* ?^?b?`?Z???T???????? */
		}

		tslp_tsk(10); /* 10msec?E?F?C?g */
	}

	/* ?X?^?[?g??m */
	ev3_led_set_color(LED_GREEN);
}

void EV3::resetSensorValue()
{
	/* ???s???[?^?[?G???R?[?_?[???Z?b?g */
	ev3_motor_reset_counts(left_motor);
	ev3_motor_reset_counts(right_motor);

	/* ?W???C???Z???T?[???Z?b?g */
	ev3_gyro_sensor_reset(gyro_sensor);
}

void EV3::playTone()
{
	ev3_speaker_play_tone(TONE_F, TONE_T);
}

void EV3::getSensorValue(int32_t* motor_ang_l, int32_t* motor_ang_r, int* color, int* gyro, int* sonic, int *volt)
{
	*motor_ang_l = ev3_motor_get_counts(left_motor);
	*motor_ang_r = ev3_motor_get_counts(right_motor);
	*color = ev3_color_sensor_get_reflect(color_sensor);
	*gyro = ev3_gyro_sensor_get_rate(gyro_sensor);
	*sonic = ev3_ultrasonic_sensor_get_distance(sonar_sensor);
	*volt = ev3_battery_voltage_mV();
}

void EV3::setMotorValue(signed char pwm_L, signed char pwm_R, signed int ang_T)
{
	if (pwm_L == 0){
		ev3_motor_stop(left_motor, true);
	}else{
		ev3_motor_set_power(left_motor, (int)pwm_L);
	}
	if (pwm_R == 0){
		ev3_motor_stop(right_motor, true);
	}else{
		ev3_motor_set_power(right_motor, (int)pwm_R);
	}
	ev3_tail_set_angle(ang_T);
}

void EV3::sleep(int sleep_sec)
{
	tslp_tsk(sleep_sec);
}

bool EV3::buttonIsPressed()
{
	return ev3_button_is_pressed(BACK_BUTTON);
}

void EV3::shutDownEV3()
{
	ev3_motor_stop(left_motor, false);
	ev3_motor_stop(right_motor, false);

	ter_tsk(BT_TASK);
	fclose(bt);
}

void EV3::ev3_tail_set_angle(signed int angle)
{
	/* ?????? */
	float pwm = (float)(angle - ev3_motor_get_counts(tail_motor))*P_GAIN;
	/* PWM?o??O?a???? */
	if (pwm > PWM_ABS_MAX){
		pwm = PWM_ABS_MAX;
	}
	else if (pwm < -PWM_ABS_MAX){
		pwm = -PWM_ABS_MAX;
	}

	if (pwm == 0){
		ev3_motor_stop(tail_motor, true);
	}
	else{
		ev3_motor_set_power(tail_motor, (signed char)pwm);
	}
}

FILE* EV3::getBT()
{
	return bt;
}

void EV3::setBTcmd(int cmd)
{
	bt_cmd = cmd;
}<file_sep>/1108_L/Phase/GaragePhase/GaragePhase.hpp
// GaragePhase.hpp

#ifndef GARAGE_PHASE_H
#define GARAGE_PHASE_H

#include "../Phase.hpp"
#include "ev3api.h"
#include <string>

class GaragePhase: public Phase {
public:
	int diff_prev=20;
  int forwardl=60;
 	int forwardr=60;
	int gyrooffset=0;
	int offset=0;
	int tailangle=3;
	int boundary=11;
	int boundary2=26;
	int cnt=0;
	int cnt2=0;
	int tim2=0;
	int pflag=0;
	int endtime;
	float Kp = 1.5;
	float Kd = 12.00;
	int mcntl=0;
	int mcntr=0;
	int LIGHT_WHITE=30;
	int LIGHT_BLACK=0;
	int data[300];
	int mcntlval;
	int mcntrval;
	int stoptime;
	FILE *fp;
	GaragePhase(std::string name, Balancer* balancer);
	GaragePhase(std::string name, Balancer* balancer,int setforwardl,int setforwardr,int setending,int gyoffset,int turnoffset,float ikp,float ikd,int itailangle, int setmcntl, int setmcntr, int setstoptime);
	void initialize();
	void calc_param(int time,int* forward,signed char* turn,int color);
	void calculate(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char* pwm_L, signed char* pwm_R, signed int* ang_T);
	bool isFinished(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char pwm_L, signed char pwm_R, signed int ang_T);
};

#endif
<file_sep>/1108_L/app.cpp
// app.cpp

#include "ev3api.h"
#include "app.hpp"
#include "app_sub.hpp"
#include "EV3/EV3.hpp"
#include "Balancer/balancer.h"
#include "Balancer/BalancerCPP.hpp"
#include "Operator/Operator.hpp"
#include "Schedule/Schedule.hpp"
#include "Phase/Phase.hpp"
#include "Phase/StartPhase/StartPhase.hpp"
#include "Phase/StraightPhase/StraightPhase.hpp"
#include "Phase/CurvePhase/CurvePhase.hpp"
#include "Phase/DetectGoal/DetectGoal.hpp"
#include "Phase/AfterGoal/AfterGoal.hpp"
#include "Phase/DetectNansho/DetectNansho.hpp"
#include "Phase/LookUpGateHandler/LookUpGateHandler.hpp"
#include "Phase/GaragePhase/GaragePhase.hpp"
#include "Phase/StairPhase/StairPhase.hpp"
#include "Phase/StairPhase2/StairPhase2.hpp"
#include "Phase/StairPhase3/StairPhase3.hpp"
#include "Phase/BeforeStair/BeforeStair.hpp"
#include "Phase/ImmediatePhase/ImmediatePhase.hpp"


//インスタンス作成
EV3 ev3;
Balancer balancer;
Schedule schedule;
Operator ope(&ev3, &balancer, &schedule);
StartPhase st("StartPhase", &balancer, 104);

/*---- stable tuning -----*/
StraightPhase sp1("StraightPhase", &balancer, 50, 70, 1000, 0);
StraightPhase sp3("StraightPhase", &balancer, 70, 70, 2800, 0); //2000 //3000

CurvePhase cp1("CurvePhase", &balancer, 60, 60, 6400, 0, 0, 1.5, 20.0); //2600

CurvePhase cp2("CurvePhase", &balancer, 70, 70, 9400, 0, 0, 1.2, 14.0); //3800

CurvePhase cp3("CurvePhase", &balancer, 55, 55, 11200, 0, 0, 2.0, 20.0);
CurvePhase cp4("CurvePhase", &balancer, 60, 60, 12200, 0, 0, 1.3, 15.0);

StraightPhase spEnd("StraightPhase", &balancer, 70, 70, 14200, 0); //2000
/*------------------------*/

//Stair
AfterGoal ags("AfterGoal", &balancer, 50, 20, 5600, 0, -2, 1.2, 15.0, 3, 92, 3000);
AfterGoal ags2("AfterGoal", &balancer, 20, 0, 1000, 0, -2, 1.2, 15.0, 92, 92, 0);

ImmediatePhase ip("ImmediatePhase", &balancer, 12, 600, 92);

GaragePhase gp2("GaragePhase", &balancer, 30, 30, 114514, 0, 0, 1.7, 0, 92, 3200, 3200, 250);
BeforeStair bs("BeforeStair", &balancer, 85);
StairPhase stair("StairPhase", &balancer);
StairPhase2 stair2("StairPhase", &balancer);
StairPhase3 stair3("StairPhase", &balancer);

//空のスケジュールにPhaseインスタンスをプッシュ
void createSchedule()
{
  schedule.pushPhase(&st);
  schedule.pushPhase(&sp1);
  schedule.pushPhase(&sp3);
  schedule.pushPhase(&cp1);
  schedule.pushPhase(&cp2);
  schedule.pushPhase(&cp3);
  schedule.pushPhase(&cp4);
  schedule.pushPhase(&spEnd);

  schedule.pushPhase(&ags);
  schedule.pushPhase(&ags2);

  schedule.pushPhase(&ip);

  schedule.pushPhase(&gp2);
  schedule.pushPhase(&bs);
  schedule.pushPhase(&stair);
  schedule.pushPhase(&stair2);
  schedule.pushPhase(&stair3);
}

/* メインタスク */
void main_task(intptr_t unused)
{
	//スケジュールを作って
	createSchedule();
	//オペレータを走らせて
	ope.run();
	//おわり
	ext_tsk();
}

/*BlueTooth*タスク*/
void bt_task(intptr_t unused)
{
  while(1)
    {
      uint8_t c = fgetc(ev3.getBT()); /* 受信 */
      switch(c)
        {
        case '1':
          ev3.setBTcmd(1);
          break;
        default:
          break;
        }
      fputc(c, ev3.getBT()); /* エコーバック */
    }
}
<file_sep>/1108_R/app.hpp
/*
 *  TOPPERS/ASP Kernel
 *      Toyohashi Open Platform for Embedded Real-Time Systems/
 *      Advanced Standard Profile Kernel
 *
 *  Copyright (C) 2000-2003 by Embedded and Real-Time Systems Laboratory
 *                              Toyohashi Univ. of Technology, JAPAN
 *  Copyright (C) 2004-2010 by Embedded and Real-Time Systems Laboratory
 *              Graduate School of Information Science, Nagoya Univ., JAPAN
 *
 *  上記著作権者は，以下の(1)?(4)の条件を満たす場合に限り，本ソフトウェ
 *  ア（本ソフトウェアを改変したものを含む．以下同じ）を使用・複製・改
 *  変・再配布（以下，利用と呼ぶ）することを無償で許諾する．
 *  (1) 本ソフトウェアをソースコードの形で利用する場合には，上記の著作
 *      権表示，この利用条件および下記の無保証規定が，そのままの形でソー
 *      スコード中に含まれていること．
 *  (2) 本ソフトウェアを，ライブラリ形式など，他のソフトウェア開発に使
 *      用できる形で再配布する場合には，再配布に伴うドキュメント（利用
 *      者マニュアルなど）に，上記の著作権表示，この利用条件および下記
 *      の無保証規定を掲載すること．
 *  (3) 本ソフトウェアを，機器に組み込むなど，他のソフトウェア開発に使
 *      用できない形で再配布する場合には，次のいずれかの条件を満たすこ
 *      と．
 *    (a) 再配布に伴うドキュメント（利用者マニュアルなど）に，上記の著
 *        作権表示，この利用条件および下記の無保証規定を掲載すること．
 *    (b) 再配布の形態を，別に定める方法によって，TOPPERSプロジェクトに
 *        報告すること．
 *  (4) 本ソフトウェアの利用により直接的または間接的に生じるいかなる損
 *      害からも，上記著作権者およびTOPPERSプロジェクトを免責すること．
 *      また，本ソフトウェアのユーザまたはエンドユーザからのいかなる理
 *      由に基づく請求からも，上記著作権者およびTOPPERSプロジェクトを
 *      免責すること．
 *
 *  本ソフトウェアは，無保証で提供されているものである．上記著作権者お
 *  よびTOPPERSプロジェクトは，本ソフトウェアに関して，特定の使用目的
 *  に対する適合性も含めて，いかなる保証も行わない．また，本ソフトウェ
 *  アの利用により直接的または間接的に生じたいかなる損害に関しても，そ
 *  の責任を負わない．
 *
 *  $Id: sample1.h 2416 2012-09-07 08:06:20Z ertl-hiro $
 */

/*
 *      サンプルプログラム(1)のヘッダファイル
 */

#ifdef __cplusplus
extern "C" {
#endif

/*
 *  ターゲット依存の定義
 */
#include "target_test.h"

/*
 *  各タスクの優先度の定義
 */

#define MAIN_PRIORITY   5           /* メインタスクの優先度 */
                                    /* HIGH_PRIORITYより高くすること */

#define HIGH_PRIORITY   9           /* 並行実行されるタスクの優先度 */
#define MID_PRIORITY    10
#define LOW_PRIORITY    11

/*
 *  ターゲットに依存する可能性のある定数の定義
 */

#ifndef TASK_PORTID
#define TASK_PORTID     1           /* 文字入力するシリアルポートID */
#endif /* TASK_PORTID */

#ifndef STACK_SIZE
#define STACK_SIZE      4096        /* タスクのスタックサイズ */
#endif /* STACK_SIZE */

/*
 *  関数のプロトタイプ宣言
 */
#ifndef TOPPERS_MACRO_ONLY

extern void main_task(intptr_t exinf);
extern void bt_task(intptr_t exinf);

#endif /* TOPPERS_MACRO_ONLY */

#ifdef __cplusplus
}
#endif
<file_sep>/1108_R/Phase/AfterGoal/AfterGoal.cpp
// AfterGoal.cpp

#include "AfterGoal.hpp"

#define LIGHT_WHITE 40
#define LIGHT_BLACK  0
#define TAIL_ANGLE_DRIVE      15

AfterGoal::AfterGoal(std::string name, Balancer* balancer) : Phase(name, balancer)
{
}

AfterGoal::AfterGoal(std::string name, Balancer* balancer,int setforwardl,int setforwardr,int setending,int gyoffset,int turnoffset,float ikp,float ikd,int setangle1, int setangle2, int setgraytime) : Phase(name, balancer)
{
	forwardl=setforwardl;
	forwardr=setforwardr;
	endtime=setending;
	gyrooffset=gyoffset;
	offset=turnoffset;
	Kp = ikp;
	Kd = ikd;
	beforeangle=setangle1;
	afterangle=setangle2;
	graytime = setgraytime;
}

void AfterGoal::initialize()
{
	fp=fopen("aftergoal.csv","w");
  if(fp==NULL){
    printf("failed\n");
    return;
  }
	fprintf(fp,"time,color,turn,gyro,volt,mtL,mtR\n");
	balancer->change_offset(gyrooffset);
}

void AfterGoal::calc_param(int time,int* forward,signed char* turn,int color){
	int diff_p = 0;
	if(time <= graytime){
		diff_p = std::min(color,LIGHT_WHITE)-(LIGHT_WHITE+15)/2;
	}else{
		diff_p = std::min(color,LIGHT_WHITE)-(LIGHT_WHITE+0)/2;
	}
	int diff_d = diff_p-(diff_prev);
	diff_prev=diff_p;
	int val=(Kp*diff_p+diff_d*Kd)/2;
	if(time==0)val=(Kp*diff_p)/2;
	(*turn)=(signed char)val;
	(*turn)+=offset;

	if(*turn<-50)
		*turn=-50;
	if(*turn>50)
		*turn=50;

}

void AfterGoal::calculate(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char* pwm_L, signed char* pwm_R, signed int* ang_T)
{
	int forward=forwardl+(forwardr-forwardl)*time/endtime;
	signed char turn;
	calc_param(time,&forward,&turn,color);
	balancer->setCommand((signed char)forward, turn);
	balancer->update(gyro, motor_ang_r, motor_ang_l, volt);
	*pwm_L = balancer->getPwmLeft();
	*pwm_R = balancer->getPwmRight();
	*ang_T = beforeangle + (afterangle - beforeangle)*time/endtime;
	fprintf(fp,"%d,%d,%d,%d,%d,%d,%d\n",time,color,(int)turn,gyro,volt,(int)*pwm_L,(int)*pwm_R);
}

bool AfterGoal::isFinished(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char pwm_L, signed char pwm_R, signed int ang_T)
{
	if (endtime==-1 && sonic<33 && sonic>=0)return true;
	if(endtime>=0 && endtime<=time)return true;
		return false;
}
<file_sep>/1108_L/Phase/Phase.cpp
// Phase.cpp

#include "Phase.hpp"

Phase::Phase(std::string name, Balancer* balancer)
{
	this->name = name;
	this->balancer = balancer;
}

std::string Phase::getName()
{
	return name;
}
<file_sep>/1108_R/Makefile.inc
mkfile_path := $(dir $(lastword $(MAKEFILE_LIST)))

# Add o.file written in C
APPL_COBJS += \
	balancer.o \
	balancer_param.o \

# Add o.file written in C++
APPL_CXXOBJS += \
	BalancerCPP.o \
	Operator.o \
	EV3.o \
	Schedule.o \
	Phase.o \
	StartPhase.o \
	StraightPhase.o \
	CurvePhase.o \
	DetectGoal.o \
	DetectNansho.o \
	LookUpGateHandler.o \
	StairPhase.o \
	StairPhase2.o \
	StairPhase3.o \
	AfterGoal.o \
	GaragePhase.o \

SRCLANG := c++

ifdef CONFIG_EV3RT_APPLICATION

# Include libraries
include $(EV3RT_SDK_LIB_DIR)/libcpp-ev3/Makefile

endif

# Add directory has .c or .cpp files
APPL_DIR += \
	 $(mkfile_path)Balancer \
	 $(mkfile_path)Operator \
	 $(mkfile_path)EV3 \
	 $(mkfile_path)Schedule \
	 $(mkfile_path)Phase \
	 $(mkfile_path)Phase/StartPhase \
	 $(mkfile_path)Phase/StraightPhase \
	 $(mkfile_path)Phase/CurvePhase \
	 $(mkfile_path)Phase/DetectGoal \
	 $(mkfile_path)Phase/DetectNansho \
	 $(mkfile_path)Phase/LookUpGateHandler \
	 $(mkfile_path)Phase/StairPhase \
	 $(mkfile_path)Phase/StairPhase2 \
	 $(mkfile_path)Phase/StairPhase3 \
	 $(mkfile_path)Phase/AfterGoal \
	 $(mkfile_path)Phase/GaragePhase \

# Add directory has .h or .hpp files
INCLUDES += \
	-I$(mkfile_path)Balancer \
	-I$(mkfile_path)Operator \
	-I$(mkfile_path)EV3 \
	-I$(mkfile_path)Schedule \
	-I$(mkfile_path)Phase \
	-I$(mkfile_path)Phase/StartPhase \
	-I$(mkfile_path)Phase/StraightPhase \
	-I$(mkfile_path)Phase/CurvePhase \
	-I$(mkfile_path)Phase/DetectGoal \
	-I$(mkfile_path)Phase/DetectNansho \
	-I$(mkfile_path)Phase/LookUpGateHandler \
	-I$(mkfile_path)Phase/StairPhase \
	-I$(mkfile_path)Phase/StairPhase2 \
	-I$(mkfile_path)Phase/StairPhase3 \
	-I$(mkfile_path)Phase/AfterGoal \
	-I$(mkfile_path)Phase/GaragePhase \
<file_sep>/1108_R/Phase/StartPhase/StartPhase.hpp
// /*TemplatePhase*/.hpp

#ifndef START_PAHSE
#define START_PHASE

#include "../Phase.hpp"
#include <string>

class StartPhase: public Phase {
public:
	StartPhase(std::string name, Balancer* balancer);
	StartPhase(std::string name, Balancer* balancer,int angle);
	void initialize();
	void calc_param(signed char* forward,signed char* turn,int color);
	void calculate(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char* pwm_L, signed char* pwm_R, signed int* ang_T);
	bool isFinished(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char pwm_L, signed char pwm_R, signed int ang_T);
private:
	int ang;
};

#endif
<file_sep>/1108_R/Phase/LookUpGateHandler/LookUpGateHandler.hpp
// /*TemplatePhase*/.hpp

#ifndef LOOK_UP_GATE_HANDLER_H
#define LOOK_UP_GATE_HANDLER_H

#include "../Phase.hpp"
#include <string>
#include <array>
#include "ev3api.h"

class LookUpGateHandler: public Phase {
public:
	LookUpGateHandler(std::string name, Balancer* balancer);
	void initialize();
	void calculate(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char* pwm_L, signed char* pwm_R, signed int* ang_T);
	bool isFinished(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char pwm_L, signed char pwm_R, signed int ang_T);
};

#endif
<file_sep>/1108_R/Phase/StairPhase2/StairPhase2.hpp
// StairPhase2.hpp

#ifndef STAIR_PHASE2_H
#define STAIR_PHASE2_H

#include "../Phase.hpp"
#include <string>

class StairPhase2: public Phase {
public:
	int diff_prev=20;
	int forwardl=60;
 	int forwardr=60;
	int gyrooffset=0;
	int endtime=5000;
	StairPhase2(std::string name, Balancer* balancer);
	StairPhase2(std::string name, Balancer* balancer,int setforwardl,int setforwardr,int setending,int gyoffset);
	void initialize();
	void calculate(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char* pwm_L, signed char* pwm_R, signed int* ang_T);
	bool isFinished(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char pwm_L, signed char pwm_R, signed int ang_T);
};

#endif<file_sep>/1108_L/Phase/ImmediatePhase/ImmediatePhase.cpp
// ImmediatePhase.cpp

#include "ImmediatePhase.hpp"

ImmediatePhase::ImmediatePhase(std::string name, Balancer* balancer) : Phase(name, balancer)
{
	/*�R���X�g���N�^*/;
}

ImmediatePhase::ImmediatePhase(std::string name, Balancer* balancer, int setforward, int settime, int setangle) : Phase(name, balancer)
{
	forward = setforward;
	endtime = settime;
	angle = setangle;

}

void ImmediatePhase::initialize()
{
	/*��������*/;
}

void ImmediatePhase::calculate(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char* pwm_L, signed char* pwm_R, signed int* ang_T)
{
	/*�Z���T�����̓��͂����ƂɃ��[�^�ւ̏o�͂��v�Z����*/
	*pwm_L = forward;
	*pwm_R = forward;
	*ang_T = angle;
}

bool ImmediatePhase::isFinished(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char pwm_L, signed char pwm_R, signed int ang_T)
{
	/*�I�������𖞂�����true,�������Ȃ�����false*/
	if(time <= endtime)
		return false;
	else
		return true;
}
<file_sep>/1108_R/Phase/CurvePhase/CurvePhase.hpp
// StraightPhase.hpp

#ifndef CURVE_PHASE_H
#define CURVE_PHASE_H

#include "../Phase.hpp"
#include <string>

class CurvePhase: public Phase {
public:
	int diff_prev=20;
	int forwardl=60;
	int forwardr=60;
	int gyrooffset=0;
	int offset=0;
	int endtime=5000;
	float Kp = 1.5;
	float Kd = 12.00;
	//FILE *fp;
	CurvePhase(std::string name, Balancer* balancer);
	CurvePhase(std::string name, Balancer* balancer,int setforwardl,int setforwardr,int setending,int gyoffset,int turnoffset,float ikp,float ikd);
	void initialize();
	void calc_param(int time,int* forward,signed char* turn,int color);
	void calculate(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char* pwm_L, signed char* pwm_R, signed int* ang_T);
	bool isFinished(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char pwm_L, signed char pwm_R, signed int ang_T);
};

#endif
<file_sep>/1108_L/Phase/StartPhase/StartPhase.cpp
// /*TemplatePhase*/.cpp

#include "StartPhase.hpp"
#define LIGHT_WHITE  40
#define LIGHT_BLACK  0
StartPhase::StartPhase(std::string name, Balancer* balancer) : Phase(name, balancer)
{
}

StartPhase::StartPhase(std::string name, Balancer* balancer,int angle) : Phase(name, balancer)
{
	ang=angle;
}

void StartPhase::initialize()
{
	balancer->change_offset(0);
}

void StartPhase::calculate(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char* pwm_L, signed char* pwm_R, signed int* ang_T)
{

	*pwm_L = 0;
	*pwm_R = 0;
	*ang_T = ang;

}

bool StartPhase::isFinished(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char pwm_L, signed char pwm_R, signed int ang_T)
{
	/*?I??????? ?????true,?????????????false*/
	if(time <= 200)
		return false;
	else
		return true;
}
<file_sep>/1108_L/Phase/Phase.hpp
// Phase.h

#ifndef PHASE_H
#define PHASE_H

#include "../Balancer/BalancerCPP.hpp"
#include <string>

class Phase {
protected:
	std::string name;
	Balancer* balancer;
public:
	Phase(std::string name, Balancer* balancer);
	std::string getName();
	virtual void initialize() = 0;
	virtual void calculate(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char* pwm_L, signed char* pwm_R, signed int* ang_T) = 0;
	virtual bool isFinished(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char pwm_L, signed char pwm_R, signed int ang_T) = 0;
};

#endif
<file_sep>/1108_R/Phase/DetectGoal/DetectGoal.hpp
// DetectGoal.hpp

#ifndef DETECTGOAL_H
#define DETECTGOAL_H

#include "../Phase.hpp"
#include <string>

class DetectGoal: public Phase {
public:
	int cnt;
	int diff_prev=20;
	int forwardl=60;
	int forwardr=60;
	int gyrooffset=0;
	int mxsptime=5000;
	int data[100];
FILE *fp;
	DetectGoal(std::string name, Balancer* balancer);
	DetectGoal(std::string name, Balancer* balancer,int setforwardl,int setforwardr,int maxspeedtime,int gyoffset);
	void initialize();
	void calc_param(int time,int* forward,signed char* turn,int color);
	void calculate(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char* pwm_L, signed char* pwm_R, signed int* ang_T);
	bool isFinished(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char pwm_L, signed char pwm_R, signed int ang_T);
};

#endif
<file_sep>/1108_L/Phase/StairPhase3/StairPhase3.cpp
// StairPhase3.cpp

#include "StairPhase3.hpp"

StairPhase3::StairPhase3(std::string name, Balancer* balancer) : Phase(name, balancer)
{
}

void StairPhase3::initialize()
{
	balancer->change_offset(0);
}

StairPhase3::StairPhase3(std::string name, Balancer* balancer,int setforwardl,int setforwardr,int setending,int gyoffset) : Phase(name, balancer)
{
	forwardl=setforwardl;
	forwardr=setforwardr;
	endtime=setending;
	gyrooffset=gyoffset;
}

void StairPhase3::calculate(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char* pwm_L, signed char* pwm_R, signed int* ang_T)
{
	if(time <= 500){
		*pwm_L = 0;
		*pwm_R = 0;
		*ang_T = 78;
	}else if(time <= 1500){
		*pwm_L = 8;
		*pwm_R = 8;
		*ang_T = 78;
	}else if(time <= 2000){
		*pwm_L = 0;
		*pwm_R = 0;
		*ang_T = 82;
	}else if(time <= 3000){
		*pwm_L = 0;
		*pwm_R = 0;
		*ang_T = 82;
	}else if(time<10000){
		*pwm_L = 25;
		*pwm_R = 25;
		*ang_T =78;
	}
}

bool StairPhase3::isFinished(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char pwm_L, signed char pwm_R, signed int ang_T)
{
	if (time <= 10800)
		return false;
	else
		return true;
}<file_sep>/1108_R/Phase/StairPhase3/StairPhase3.cpp
// StairPhase3.cpp

#include "StairPhase3.hpp"

StairPhase3::StairPhase3(std::string name, Balancer* balancer) : Phase(name, balancer)
{
}

void StairPhase3::initialize()
{
	balancer->change_offset(0);
}

StairPhase3::StairPhase3(std::string name, Balancer* balancer,int setforwardl,int setforwardr,int setending,int gyoffset) : Phase(name, balancer)
{
	forwardl=setforwardl;
	forwardr=setforwardr;
	endtime=setending;
	gyrooffset=gyoffset;
}

void StairPhase3::calculate(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char* pwm_L, signed char* pwm_R, signed int* ang_T)
{
	if(time <= 1000){
		*pwm_L = 0;
		*pwm_R = 0;
		*ang_T = 78;
	}else if(time <= 2000){
		*pwm_L = 0;
		*pwm_R = 0;
		*ang_T = 88;
	}else if(time <= 3000){
		*pwm_L = 0;
		*pwm_R = 0;
		*ang_T = 91;
	}else if(time <= 4000){
		*pwm_L = 0;
		*pwm_R = 0;
		*ang_T = 94;
	}else if(time <= 5000){
		*pwm_L = 0;
		*pwm_R = 0;
		*ang_T = 96;
	}else if(time <= 6000){
		*pwm_L = 0;
		*pwm_R = 0;
		*ang_T = 98;
	}else if(time <= 7000){
		*pwm_L = 0;
		*pwm_R = 0;
		*ang_T = 100;
	}else if(time <= 30000){
		balancer->setCommand(15, 0);
		balancer->update(gyro, motor_ang_r, motor_ang_l, volt);
		*pwm_L = balancer->getPwmRight();
		*pwm_R = balancer->getPwmLeft();
		*ang_T = 30;
	}

}

bool StairPhase3::isFinished(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char pwm_L, signed char pwm_R, signed int ang_T)
{
	/*終了条件を満たせばtrue,満たさなければfalse*/
	if (time <= 30000)
		return false;
	else
		return true;
}
<file_sep>/1108_L/Phase/DetectNansho/DetectNansho.cpp
// /*DetectNansho*/.cpp
// setting-> offset: +10, CMD_MAX=60
#include "DetectNansho.hpp"

#define LIGHT_WHITE  40
#define LIGHT_BLACK  0
#define TAIL_ANGLE_DRIVE      3

DetectNansho::DetectNansho(std::string name, Balancer* balancer) : Phase(name, balancer)
{
}

void DetectNansho::initialize()
{
	balancer->change_offset(0);
	fp=fopen("log.csv","w");
  if(fp==NULL){
    printf("failed\n");
    return;
  }
	fprintf(fp,"time,color\n");
	cnt=0;
	for(int i=0;i<100;i++){
		data[i]=0;
	}
}

void DetectNansho::calc_param(signed char* forward,signed char* turn,int color){
	if(color>=(LIGHT_WHITE+LIGHT_BLACK)/2*1.7){
		*turn = 10;
	}else if(color>=(LIGHT_WHITE+LIGHT_BLACK)/2){
		*turn=2;
	}else{
		*turn=-2;
	}
}

void DetectNansho::calculate(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char* pwm_L, signed char* pwm_R, signed int* ang_T)
{
	signed char forward;
	signed char turn;
	if(time<500)forward=45;
	else forward=25;
	calc_param(&forward,&turn,color);
	fprintf(fp,"%d,%d,%d,%d\n",time,color,(int)forward,(int)turn);
	balancer->setCommand(forward, turn);
	balancer->update(gyro, motor_ang_r, motor_ang_l, volt);
	*pwm_L = balancer->getPwmRight();
	*pwm_R = balancer->getPwmLeft();
	*ang_T = 80;
}

bool DetectNansho::isFinished(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char pwm_L, signed char pwm_R, signed int ang_T)
{
	int Graymin=17;
	int Graymax=27;
	int tim=time/4;
	if(tim>=1000){
			if(tim>=1100){
					cnt-=(data[tim%100]>=Graymin && data[tim%100]<=Graymax)?1:0;
			}
			data[tim%100]=color;
			cnt+=(data[tim%100]>=Graymin && data[tim%100]<=Graymax)?1:0;
			if(tim>=1100 && cnt>=50){
					return true;
			}
	}
	return false;
}
<file_sep>/1108_L/Phase/ImmediatePhase/ImmediatePhase.hpp
// ImmediatePhase.hpp

#ifndef IMMEDIATE_PHASE_H
#define IMMEDIATE_PHASE_H

#include "../Phase.hpp"
#include <string>

class ImmediatePhase: public Phase {
public:
	int forward;
	int endtime;
	int angle;
	ImmediatePhase(std::string name, Balancer* balancer);
	ImmediatePhase(std::string name, Balancer* balancer, int setforward, int settime, int setangle);

	void initialize();
	void calculate(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char* pwm_L, signed char* pwm_R, signed int* ang_T);
	bool isFinished(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char pwm_L, signed char pwm_R, signed int ang_T);
};

#endif
<file_sep>/1108_R/Phase/TightCurve/TightCurve.cpp
// TightCurve.cpp

#include "TightCurve.hpp"

#define LIGHT_WHITE  40
/* 黒色の光センサ値 */
#define LIGHT_BLACK  0
/* バランス走行時の角度[度] */
#define TAIL_ANGLE_DRIVE      3

TightCurve::TightCurve(std::string name, Balancer* balancer) : Phase(name, balancer)
{
}

void TightCurve::initialize()
{
	fp=fopen("tightcurvelog.csv","w");
  if(fp==NULL){
    printf("failed\n");
    return;
  }
	fprintf(fp,"time,color,turn,gyro,volt,mtL,mtR\n");
	balancer->change_offset(gyrooffset);
}

TightCurve::TightCurve(std::string name, Balancer* balancer,int setforwardl,int setforwardr,int setending,int gyoffset) : Phase(name, balancer)
{
	forwardl=setforwardl;
	forwardr=setforwardr;
	endtime=setending;
	gyrooffset=gyoffset;
}

void TightCurve::calc_param(int time,int* forward,signed char* turn,int color){
	const float Kp = 1.13;
	const float Kd = 10.00;
	int offset=-2;
	int diff_p = std::min(color,LIGHT_WHITE)-(LIGHT_WHITE+LIGHT_BLACK)/2;
	int diff_d = diff_p-(diff_prev);
	diff_prev=diff_p;
	int val=(Kp*diff_p+diff_d*Kd)/2;
	if(time==0)val=(Kp*diff_p)/2;
	(*turn)=(signed char)val;
	(*turn)+=offset;
	(*turn)*=-1;
	if(*turn<-12){
		*turn=-12;
	}
	if(*turn>12){
		*turn=12;
	}
}

void TightCurve::calculate(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char* pwm_L, signed char* pwm_R, signed int* ang_T)
{
	/*センサからの入力をもとにモータへの出力を計算する*/
	/*turn = 1の2値制御*/
	int forward=forwardl+(forwardr-forwardl)*time/endtime;
	signed char turn;
	calc_param(time,&forward,&turn,color);
	balancer->setCommand((signed char)forward, turn);
	balancer->update(gyro, motor_ang_r, motor_ang_l, volt);
	*pwm_L = balancer->getPwmRight();
	*pwm_R = balancer->getPwmLeft();
	*ang_T = TAIL_ANGLE_DRIVE;
	fprintf(fp,"%d,%d,%d,%d,%d,%d,%d\n",time,color,(int)turn,gyro,volt,(int)*pwm_L,(int)*pwm_R);
}

bool TightCurve::isFinished(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char pwm_L, signed char pwm_R, signed int ang_T)
{
	/*終了条件を満たせばtrue,満たさなければfalse*/
	if (time <= endtime)
		return false;
	else
		return true;
}
<file_sep>/1108_L/Phase/_TemplatePhase/TemplatePhase.cpp
// /*TemplatePhase*/.cpp

#include "/*TemplatePhase*/.hpp"

/*TemplatePhase*/::/*TemplatePhase*/(std::string name, Balancer* balancer) : Phase(name, balancer)
{
	/*コンストラクタ*/;
}

void /*TemplatePhase*/::initialize()
{
	/*初期処理*/;
}

void /*TemplatePhase*/::calculate(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char* pwm_L, signed char* pwm_R, signed int* ang_T)
{
	/*センサからの入力をもとにモータへの出力を計算する*/
	*pwm_L = 0;
	*pwm_R = 0;
	*ang_T = 0;
}

bool /*TemplatePhase*/::isFinished(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char pwm_L, signed char pwm_R, signed int ang_T)
{
	/*終了条件を満たせばtrue,満たさなければfalse*/
	if(1)
		return false;
	else
		return true;
}
<file_sep>/1108_R/EV3/EV3.hpp
// EV3.h

#ifndef EV3_H
#define EV3_H

#include "ev3api.h"

class EV3 {
private:
	void ev3_tail_set_angle(signed int angle);
public:
	void setUpEV3();
	void waitForStart();
	void resetSensorValue();
	void playTone();
	void getSensorValue(int32_t* motor_ang_l, int32_t* motor_ang_r, int* color, int* gyro, int* sonic, int* volt);
	void setMotorValue(signed char pwm_L, signed char pwm_R, signed int ang_T);
	void sleep(int sleep_sec);
	bool buttonIsPressed();
	void shutDownEV3();
	FILE* getBT();
	void setBTcmd(int cmd);
};

#endif
<file_sep>/1108_L/Operator/Operator.hpp
// Operator.h

#ifndef OPERATOR_H
#define OPERATOR_H

#include "../EV3/EV3.hpp"
#include "../Balancer/BalancerCPP.hpp"
#include "../Schedule/Schedule.hpp"
#include "../Phase/Phase.hpp"

class Operator {
private:
	EV3* ev3;
	Balancer* balancer;
	Schedule* schedule;
public:
	Operator(EV3* ev3, Balancer* balancer, Schedule* schedule);
	void run();
};

#endif
<file_sep>/1108_R/Schedule/Schedule.cpp
// Schedule.cpp

#include "Schedule.hpp"

void Schedule::pushPhase(Phase* phase)
{
	queue.push(phase);
}

Phase* Schedule::popPhase()
{
	Phase* phase = queue.front();
	queue.pop();
	return phase;
}

bool Schedule::isEmpty()
{
	return !queue.empty();
}
<file_sep>/1108_R/Schedule/Schedule.hpp
// Schedule.h

#ifndef SCHEDULE_H
#define SCHEDULE_H

#include "../Phase/Phase.hpp"
#include <queue>

class Schedule {
private: 
	std::queue<Phase*> queue;
public:
	void pushPhase(Phase* phase);
	Phase* popPhase();
	bool isEmpty();
};

#endif
<file_sep>/1108_L/Phase/LookUpGateHandler/LookUpGateHandler.cpp
#include "LookUpGateHandler.hpp"

#define TAIL_ANGLE_STAND_UP 93
#define TAIL_ANGLE_DRIVE 15
#define TAIL_ANGLE_LUG 70
#define MOTOR_FORWARD 7
#define MOTOR_STOP 0
#define MOTOR_FORWARD_SLOW 2
#define MOTOR_BACKWARD_SLOW -2
#define TIME_SECTION 5175
#define LINE_TRACE 4
#define LIGHT_WHITE 6
#define LIGHT_BLACK 0
#define MUSIC_DURATION 500
#define MUSIC_ELEMENT 16

std::array<int, 9> tail_down{93, 90, 87, 84, 81, 78, 75, 72, 70};
std::array<int, 6> tail_up{70, 75, 80, 85, 89, 92};

double tone_array[MUSIC_ELEMENT] = {NOTE_C4, NOTE_C4, NOTE_G4, NOTE_G4, NOTE_A4, NOTE_A4, NOTE_G4, NOTE_G4, NOTE_F4, NOTE_F4, NOTE_E4, NOTE_E4, NOTE_D4, NOTE_D4, NOTE_C4, NOTE_C4};

LookUpGateHandler::LookUpGateHandler(std::string name, Balancer* balancer) : Phase(name, balancer)
{

}

void LookUpGateHandler::initialize()
{

}
//修正前 ~1：前進小, ~2.0：前進大, ~3.0：回転, ~5.0：前進, ~6.0：回転, ~8.0：前進
void LookUpGateHandler::calculate(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char* pwm_L, signed char* pwm_R, signed int* ang_T)
{
	if(time < 0.2 * TIME_SECTION){
		*pwm_L = MOTOR_FORWARD + 3;
		*pwm_R = MOTOR_FORWARD + 3;
		//*ang_T = tail_down[time / (TIME_SECTION / DIVISION)];
	}else if(time < 1 * TIME_SECTION){
		*pwm_L = MOTOR_FORWARD_SLOW;
		*pwm_R = MOTOR_FORWARD_SLOW;
		*ang_T = tail_down[time / (TIME_SECTION / tail_down.size())]; //DIVISION
	}else if(time < 20*TIME_SECTION/10){
		*pwm_L = MOTOR_FORWARD;
		*pwm_R = MOTOR_FORWARD;
		if(color > 2 * (LIGHT_WHITE + LIGHT_BLACK) / 3){
			*pwm_R = *pwm_R - LINE_TRACE;
		}else if(color < 1 * (LIGHT_WHITE + LIGHT_BLACK) / 3){
			*pwm_L = *pwm_L - LINE_TRACE;
		}
		*ang_T = TAIL_ANGLE_LUG;
	}else if(time < 30 * TIME_SECTION/10){
		*pwm_L = 6;
		*pwm_R = -6;
		*ang_T = TAIL_ANGLE_LUG;
	}else if(time < 50 * TIME_SECTION/10){
		*pwm_L = MOTOR_FORWARD;
		*pwm_R = MOTOR_FORWARD;
		if(color > 2 * (LIGHT_WHITE + LIGHT_BLACK) / 3){
			*pwm_L = *pwm_L - LINE_TRACE;
		}else if(color < 1 * (LIGHT_WHITE + LIGHT_BLACK) / 3){
			*pwm_R = *pwm_R - LINE_TRACE;
		}
		*ang_T = TAIL_ANGLE_LUG;
	}else if(time < 60 * TIME_SECTION/10){
		*pwm_L = 6;
		*pwm_R = -6;
		*ang_T = TAIL_ANGLE_LUG;
	}else if(time < 80 * TIME_SECTION/10){
		*pwm_L = MOTOR_FORWARD;
		*pwm_R = MOTOR_FORWARD;
		if(color > 2 * (LIGHT_WHITE + LIGHT_BLACK) / 3){
			*pwm_R = *pwm_R - LINE_TRACE;
		}else if(color < 1 * (LIGHT_WHITE + LIGHT_BLACK) / 3){
			*pwm_L = *pwm_L - LINE_TRACE;
		}
		*ang_T = TAIL_ANGLE_LUG;
	}else{
		*pwm_L = MOTOR_BACKWARD_SLOW;
		*pwm_R = MOTOR_BACKWARD_SLOW;
		*ang_T = tail_up[(time - 80 * TIME_SECTION/10) / (TIME_SECTION / tail_up.size())]; // DIVISION
	}
	//if(time % MUSIC_DURATION == 0) ev3_speaker_play_tone(tone_array[(time / MUSIC_DURATION) % MUSIC_ELEMENT], MUSIC_DURATION);
}

bool LookUpGateHandler::isFinished(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char pwm_L, signed char pwm_R, signed int ang_T)
{
	if(time < 90 * TIME_SECTION/10) return false;
	else return true;
}
<file_sep>/1108_L/Phase/StraightPhase/StraightPhase.cpp
// StraightPhase.cpp

#include "StraightPhase.hpp"

#define LIGHT_WHITE  40
/* ???F????Z???T?l */
#define LIGHT_BLACK  0
/* ?o?????X???s????p?x[?x] */
#define TAIL_ANGLE_DRIVE      15

StraightPhase::StraightPhase(std::string name, Balancer* balancer) : Phase(name, balancer)
{
}

StraightPhase::StraightPhase(std::string name, Balancer* balancer,int setforwardl,int setforwardr,int setending,int gyoffset) : Phase(name, balancer)
{
	forwardl=setforwardl;
	forwardr=setforwardr;
	endtime=setending;
	gyrooffset=gyoffset;
}

void StraightPhase::initialize()
{
	fp=fopen("straightlog.csv","w");
  if(fp==NULL){
    printf("failed\n");
    return;
  }
	fprintf(fp,"time,color,turn,gyro,volt,mtL,mtR\n");

	balancer->change_offset(gyrooffset);
	for(int i = 0; i < 50; i++){
		pwm_L_list[i] = 0;
		pwm_R_list[i] = 0;
	}
}

void StraightPhase::calc_param(int time,int* forward,signed char* turn,int color){
	const float Kp = 1.1;
	const float Kd = 10.00;
	int diff_p = std::min(color,LIGHT_WHITE)-(LIGHT_WHITE+LIGHT_BLACK)/2;
	/*
	if(time>=200){
		int va=0;
		for(int i=0;i<4;i++){
			va+=colordi[i];
		}
		va/=50;
		colordi[(time/4)%50]=diff_p;
		diff_p=va;
	}
	*/
	int diff_d = diff_p-(diff_prev);
	diff_prev=diff_p;
	int val=(Kp*diff_p+diff_d*Kd)/2;
	if (time == 0) val = (Kp * diff_p) / 2;
	(*turn)=(signed char)val;
	if(*turn >= 50)
		*turn = 50;
	if(*turn <= -50)
		*turn = -50;
}

void StraightPhase::calculate(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char* pwm_L, signed char* pwm_R, signed int* ang_T)
{
	int forward=forwardl+(forwardr-forwardl)*motor_ang_l/endtime;
	signed char turn;
	calc_param(time,&forward,&turn,color);
	balancer->setCommand((signed char)forward, turn);
	balancer->update(gyro, motor_ang_r, motor_ang_l, volt);
	*pwm_L = balancer->getPwmLeft();
	*pwm_R = balancer->getPwmRight();
	*ang_T = TAIL_ANGLE_DRIVE;
	fprintf(fp,"%d,%d,%d,%d,%d,%d,%d\n",time,color,(int)turn,gyro,volt, (int)motor_ang_l,(int)motor_ang_r);
}

bool StraightPhase::isFinished(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char pwm_L, signed char pwm_R, signed int ang_T)
{
	pwm_L_list[(time/4)%20] = pwm_L;
	pwm_R_list[(time/4)%20] = pwm_R;
	int counts = 0;
	for(int i = 0; i < 20; i++){
		if(pwm_L_list[i] - pwm_R_list[i] >= 20)
			counts++;
	}

	// if (time > endtime){
	// 	return true;
	// }else if(time >= 3000 && counts >= 19){
	// 	return true;
	// }else {
	// 	return false;
	// }

	if (motor_ang_l > endtime){
		return true;
	} else {
		return false;
	}
}
<file_sep>/1108_R/app_sub.hpp
#if defined(BUILD_MODULE)
#include "module_cfg.h"
#else
#include "kernel_cfg.h"

#endif

#define DEBUG

#ifdef DEBUG
#define _debug(x) (x)
#else
#define _debug(x)

#endif
<file_sep>/1108_L/Phase/GaragePhase/GaragePhase.cpp
// GaragePhase.cpp

#include "GaragePhase.hpp"
#define TIME_BAND   250
#define TAIL_ANGLE_DRIVE   15

GaragePhase::GaragePhase(std::string name, Balancer* balancer,int setforwardl,int setforwardr,int setending,int gyoffset,int turnoffset,float ikp,float ikd,int itailangle, int setmcntl, int setmcntr, int setstoptime) : Phase(name, balancer)
{
	forwardl=setforwardl;
	forwardr=setforwardr;
	endtime=setending;
	gyrooffset=gyoffset;
	offset=turnoffset;
	Kp = ikp;
	Kd = ikd;
	tailangle=itailangle;
	mcntlval=setmcntl;
	mcntrval=setmcntr;
	stoptime=setstoptime;
}

void GaragePhase::initialize()
{
	fp=fopen("garagephase.csv","w");
  if(fp==NULL){
    printf("failed\n");
    return;
  }
	fprintf(fp,"time,color,turn,gyro,volt,mtL,mtR,flag,mcntl,mcntr\n");
	balancer->change_offset(gyrooffset);
}

void GaragePhase::calc_param(int time,int* forward,signed char* turn,int color){
	int diff_p = std::min(color,LIGHT_WHITE)-(LIGHT_WHITE+LIGHT_BLACK/*+offset*/)/2;
	int diff_d = diff_p-(diff_prev);
	diff_prev=diff_p;
	int val=(Kp*diff_p+diff_d*Kd)/2;
	if(time==0)val=(Kp*diff_p)/2;
	(*turn)=(signed char)val;
	if(*turn<=-2){
		*turn=-2;
	}
	if(*turn>=2){
		*turn=2;
	}
}


void GaragePhase::calculate(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char* pwm_L, signed char* pwm_R, signed int* ang_T)
{
	int forward=forwardl+(forwardr-forwardl)*time/endtime;
	signed char turn;
	calc_param(time,&forward,&turn,color);
	*pwm_L = 7;
	*pwm_R = 7;
	if(turn>0)*pwm_R-=turn;
	if(turn<0)*pwm_L+=turn;
	*ang_T = tailangle;
	if(pflag==2){
		*pwm_R=0;
		*pwm_L=0;
	}
	fprintf(fp,"%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n",time,color,(int)turn,gyro,volt,(int)*pwm_L,(int)*pwm_R,pflag,mcntl,mcntr);
}

bool GaragePhase::isFinished(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char pwm_L, signed char pwm_R, signed int ang_T)
{
	int tim = time / 4;

	if (time >= 2500) {
		int idx = tim % TIME_BAND;

		if (time >= 3500 && data[idx] <= boundary) {
			cnt--;
		}
		data[idx] = color;
		if (data[idx] <= boundary) cnt++;
	}
	if(time>=3500 && cnt>210 && pflag==0){
		pflag=1;
		ev3_speaker_play_tone(50, 1000);
	}
	if(time>=6500 && pflag==0){
		int val=0;
		for(int i=0;i<5;i++){
			val+=data[(tim-i+TIME_BAND)%TIME_BAND];
			val-=data[(tim+i+1)%TIME_BAND];
		}
		if(val>=40)pflag=1;
	}
	if(pflag>=1){
		mcntl+=(int)pwm_L;
		mcntr+=(int)pwm_R;
	}
	if(pflag==1 && mcntl>=mcntlval && mcntr>=mcntrval){//Garage：mcntlval=5200, mcntrval=5200
		pflag=2;
		ev3_speaker_play_tone(70, 1000);
	}
	if(pflag==2)tim2++;
	if(tim2>=stoptime)return true;//Garage：stoptime=2000
	return false;
}
<file_sep>/1108_R/app.cpp
// app.cpp

#include "ev3api.h"
#include "app.hpp"
#include "app_sub.hpp"
#include "EV3/EV3.hpp"
#include "Balancer/balancer.h"
#include "Balancer/BalancerCPP.hpp"
#include "Operator/Operator.hpp"
#include "Schedule/Schedule.hpp"
#include "Phase/Phase.hpp"
#include "Phase/StartPhase/StartPhase.hpp"
#include "Phase/StraightPhase/StraightPhase.hpp"
#include "Phase/CurvePhase/CurvePhase.hpp"
#include "Phase/DetectGoal/DetectGoal.hpp"
#include "Phase/AfterGoal/AfterGoal.hpp"
#include "Phase/DetectNansho/DetectNansho.hpp"
#include "Phase/LookUpGateHandler/LookUpGateHandler.hpp"
#include "Phase/StairPhase/StairPhase.hpp"
#include "Phase/StairPhase2/StairPhase2.hpp"
#include "Phase/StairPhase3/StairPhase3.hpp"
#include "Phase/GaragePhase/GaragePhase.hpp"


//インスタンス作成
EV3 ev3;
Balancer balancer;
Schedule schedule;
Operator ope(&ev3, &balancer, &schedule);
StartPhase st("StartPhase", &balancer, 106);

/*---- stable tuning -----*/
StraightPhase sp1("StraightPhase", &balancer, 50, 70, 1000, 0);
StraightPhase sp3("StraightPhase", &balancer, 70, 70, 2500, 0); //1500

//CurvePhase cp1("CurvePhase", &balancer, 70, 70, 7500, 0, 0, 1.6, 18.0); //4500
CurvePhase cp1("CurvePhase", &balancer, 70, 70, 9000, 0, 0, 1.4, 15.0); //5200
CurvePhase cp2("CurvePhase", &balancer, 60, 60, 10300, 0, 0, 1.8, 20.0); //2600
CurvePhase cp3("CurvePhase", &balancer, 70, 70, 12500, 0, 0, 1.4, 16.0); //1600

StraightPhase spEnd("StraightPhase", &balancer, 70, 70, 14500, 0); //2600
/*------------------------*/
DetectGoal dg("DetectGoal", &balancer,70, 75,1300,0);

AfterGoal ag("AfterGoal", &balancer, 45, 30, 5800, 0, -2, 1.2, 15.0, 3, 3, 3000);
AfterGoal ag2("AfterGoal", &balancer, 30, 30, -1, 0, -2, 1.2, 15.0, 3, 3, 114514);
AfterGoal ag3("AfterGoal", &balancer, 30, 0, 1000, 0, 5, 1.2, 15.0, 3, 93, 0);
AfterGoal ag4("AfterGoal", &balancer, 0, 0, 1000, 0, 5, 1.2, 15.0, 93, 93, 0);
LookUpGateHandler lug("LookUpGateHandler", &balancer);

//GaragePhase gp("GaragePhase", &balancer,30,30,114514,0,0,1.7,0,92);
GaragePhase gp("GaragePhase", &balancer, 30, 30, 114514, 0, 0, 1.7, 0, 92, 5200, 5200, 2000);

//空のスケジュールにPhaseインスタンスをプッシュ
void createSchedule()
{
  schedule.pushPhase(&st);
  schedule.pushPhase(&sp1);
  schedule.pushPhase(&sp3);
  schedule.pushPhase(&cp1);
  schedule.pushPhase(&cp2);
  schedule.pushPhase(&cp3);
  schedule.pushPhase(&spEnd);
//  schedule.pushPhase(&dg);
  schedule.pushPhase(&ag);
  schedule.pushPhase(&ag2);
  schedule.pushPhase(&ag3);
  schedule.pushPhase(&ag4);
  schedule.pushPhase(&lug);
  schedule.pushPhase(&gp);
}

/* メインタスク */
void main_task(intptr_t unused)
{
	//スケジュールを作って
	createSchedule();
	//オペレータを走らせて
	ope.run();
	//おわり
	ext_tsk();
}

/*BlueTooth*タスク*/
void bt_task(intptr_t unused)
{
  while(1)
    {
      uint8_t c = fgetc(ev3.getBT()); /* 受信 */
      switch(c)
        {
        case '1':
          ev3.setBTcmd(1);
          break;
        default:
          break;
        }
      fputc(c, ev3.getBT()); /* エコーバック */
    }
}
<file_sep>/1108_L/Phase/BeforeStair/BeforeStair.hpp
// BeforeStair.hpp

#ifndef BEFORE_STAIR_H
#define BEFORE_STAIR_H

#include "../Phase.hpp"
#include <string>

class BeforeStair: public Phase {
public:

	int angle;

	FILE *fp;
	BeforeStair(std::string name, Balancer* balancer);
	BeforeStair(std::string name, Balancer* balancer,int tailangle);
	void initialize();
	void calc_param(int time,int* forward,signed char* turn,int color);
	void calculate(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char* pwm_L, signed char* pwm_R, signed int* ang_T);
	bool isFinished(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char pwm_L, signed char pwm_R, signed int ang_T);
};

#endif
<file_sep>/1108_L/Phase/BeforeStair/BeforeStair.cpp
// BeforeStair.cpp

#include "BeforeStair.hpp"



#define LIGHT_WHITE 25
#define LIGHT_BLACK 0
#define TURN_MAX 7


BeforeStair::BeforeStair(std::string name, Balancer* balancer) : Phase(name, balancer)
{
	/*�R���X�g���N�^*/;
}

BeforeStair::BeforeStair(std::string name, Balancer* balancer, int tailangle) : Phase(name, balancer)
{

	angle = tailangle;
}




void BeforeStair::initialize()
{
	/*��������*/
	fp = fopen("BeforeStair.csv", "w");
	if (fp == NULL) {
		printf("failed\n");
		return;
	}
	fprintf(fp, "time,color,mtL,mtR\n");
}


void BeforeStair::calculate(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char* pwm_L, signed char* pwm_R, signed int* ang_T)
{
		//�ҋ@
	if (time <= 3000) {
		*pwm_L = 0;
		*pwm_R = 0;
		*ang_T = angle;
		//���]���Č��������ɂȂ�
	} else if (time <= (5125 + 3000)) {
		*pwm_L = 6;
		*pwm_R = -6;
		*ang_T = angle;
	} else if (time <= (5125 + 5000)) {
		*pwm_L = 0;
		*pwm_R = 0;
		*ang_T = angle;



	}
	fprintf(fp, "%d,%d,%d,%d\n", time, color, (int)*pwm_L, (int)*pwm_R);

}

bool BeforeStair::isFinished(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char pwm_L, signed char pwm_R, signed int ang_T)
{
	/*�I�������𖞂�����true,�������Ȃ�����false*/
	if (12000 >= time)//17000
		return false;
	else
		return true;
}
<file_sep>/1108_R/Phase/TightCurve/TightCurve.hpp
// StraightPhase.hpp

#ifndef TIGHT_CURVE_H
#define TIGHT_CURVE_H

#include "../Phase.hpp"
#include <string>
#include <cmath>
#include <algorithm>

class TightCurve: public Phase {
public:
	int diff_prev=20;
  int forwardl=60;
 	int forwardr=60;
	int gyrooffset=0;
	int endtime=5000;
	FILE *fp;
	TightCurve(std::string name, Balancer* balancer);
	TightCurve(std::string name, Balancer* balancer,int setforwardl,int setforwardr,int setending,int gyoffset);
	void initialize();
	void calc_param(int time,int* forward,signed char* turn,int color);
	void calculate(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char* pwm_L, signed char* pwm_R, signed int* ang_T);
	bool isFinished(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char pwm_L, signed char pwm_R, signed int ang_T);
};

#endif
<file_sep>/1108_L/Phase/StairPhase/StairPhase.cpp
// StairPhase.cpp

#include "StairPhase.hpp"

StairPhase::StairPhase(std::string name, Balancer* balancer) : Phase(name, balancer)
{
}

void StairPhase::initialize()
{
	balancer->change_offset(0);
}

StairPhase::StairPhase(std::string name, Balancer* balancer,int setforwardl,int setforwardr,int setending,int gyoffset) : Phase(name, balancer)
{
	forwardl=setforwardl;
	forwardr=setforwardr;
	endtime=setending;
	gyrooffset=gyoffset;
}

void StairPhase::calculate(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char* pwm_L, signed char* pwm_R, signed int* ang_T)
{
	//EV3��2���ɕ����ĐQ����
	if(time <= 2000){
		*pwm_L = 0;
		*pwm_R = 0;
		*ang_T = 80;

	//�i���ɐڋ�
	}else if(time <= 4000){
		*pwm_L = -8;
		*pwm_R = -8;
		*ang_T = 80;

	//�K���̉ғ��͈͊m��
	}else if(time <= 4600){
		*pwm_L = 8;
		*pwm_R = 8;
		*ang_T = 80;

	//EV3���R���ɕ����ė��Ă�
	}else if(time <= 6800){
		*pwm_L = 0;
		*pwm_R = 0;
		*ang_T = 83;
	}else if(time <= 7800){
		*pwm_L = 0;
		*pwm_R = 0;
		if(volt >= 8600){
			*ang_T = 83;
		}else if(volt >= 8400){
			*ang_T = 85;
		}else {
			*ang_T = 86;
		}

	//�i��������
	}else if(time <= 8000){
		*pwm_L = -20;
		*pwm_R = -20;
		*ang_T = 40+40*((time-7800)/200);
	}else if(time <= 9200){
		*pwm_L = -20;
		*pwm_R = -20;
		*ang_T = 80;
	}else if(time <= 12000){
		*pwm_L = 0;
		*pwm_R = 0;
		*ang_T = 80;

	}else if(time <= 15120){
		*pwm_L = 20;
		*pwm_R = -20;
		*ang_T = 80;
	}

}

bool StairPhase::isFinished(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char pwm_L, signed char pwm_R, signed int ang_T)
{
	/*�I�������𖞂�����true,�������Ȃ�����false*/
	if (time <= 15120)
		return false;
	else
		return true;
}
<file_sep>/1108_L/Phase/CurvePhase/CurvePhase.cpp
// CurvePhase.cpp

#include "CurvePhase.hpp"

#define LIGHT_WHITE 40
#define LIGHT_BLACK  0
#define TAIL_ANGLE_DRIVE      15

CurvePhase::CurvePhase(std::string name, Balancer* balancer) : Phase(name, balancer)
{
}

CurvePhase::CurvePhase(std::string name, Balancer* balancer,int setforwardl,int setforwardr,int setending,int gyoffset,int turnoffset,float ikp,float ikd) : Phase(name, balancer)
{
	forwardl=setforwardl;
	forwardr=setforwardr;
	endtime=setending;
	gyrooffset=gyoffset;
	offset=turnoffset;
	Kp = ikp;
	Kd = ikd;
}

void CurvePhase::initialize()
{
/*
	char fine_name[] = "";
	strcat(fine_name, name.c_str());
	strcat(fine_name, ".csv");
	fp=fopen(fine_name,"w");
  if(fp==NULL){
    printf("failed\n");
    return;
  }
	fprintf(fp,"time,color,turn,gyro,volt,mtL,mtR\n");
*/
	balancer->change_offset(gyrooffset);
}

void CurvePhase::calc_param(int time,int* forward,signed char* turn,int color){
	int diff_p = std::min(color,LIGHT_WHITE)-(LIGHT_WHITE+LIGHT_BLACK+offset)/2;
	int diff_d = diff_p-(diff_prev);
	diff_prev=diff_p;
	int val=(Kp*diff_p+diff_d*Kd)/2;
	if(time==0)val=(Kp*diff_p)/2;
	(*turn)=(signed char)val;
	if(*turn >= 60)
		*turn = 60;
	if(*turn <= -60)
		*turn = -60;
}

void CurvePhase::calculate(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char* pwm_L, signed char* pwm_R, signed int* ang_T)
{
	/*?Z???T?????????????????[?^???o????v?Z????*/
	/*turn = 1??2?l????*/
	int forward=forwardl+(forwardr-forwardl)*time/endtime;
	signed char turn;
	calc_param(time,&forward,&turn,color);
	balancer->setCommand((signed char)forward, turn);
	balancer->update(gyro, motor_ang_r, motor_ang_l, volt);
	*pwm_L = balancer->getPwmLeft();
	*pwm_R = balancer->getPwmRight();
	*ang_T = TAIL_ANGLE_DRIVE;
	//fprintf(fp,"%d,%d,%d,%d,%d,%d,%d\n",time,color,(int)turn,gyro,volt,(int)*pwm_L,(int)*pwm_R);
}

bool CurvePhase::isFinished(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char pwm_L, signed char pwm_R, signed int ang_T)
{
	if (motor_ang_l > endtime)
		return true;
	else
		return false;
}
<file_sep>/1108_R/Phase/AfterGoal/AfterGoal.hpp
// AfterGaol.hpp

#ifndef AFTER_GOAL_H
#define AFTER_GOAL_H

#include "../Phase.hpp"
#include <string>

class AfterGoal: public Phase {
public:
	int diff_prev=20;
	int forwardl=60;
	int forwardr=60;
	int gyrooffset=0;
	int offset=0;
	//int tailangle=3;
	int beforeangle;
	int afterangle;
	int endtime=5000;
	float Kp = 1.5;
	float Kd = 12.00;
	int graytime = 0;
	FILE *fp;
	AfterGoal(std::string name, Balancer* balancer);
	AfterGoal(std::string name, Balancer* balancer,int setforwardl,int setforwardr,int setending,int gyoffset,int turnoffset,float ikp,float ikd,int setangle1, int setangle2,int setgraytime);
	void initialize();
	void calc_param(int time,int* forward,signed char* turn,int color);
	void calculate(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char* pwm_L, signed char* pwm_R, signed int* ang_T);
	bool isFinished(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char pwm_L, signed char pwm_R, signed int ang_T);
};

#endif
<file_sep>/1108_L/Phase/DetectGoal/DetectGoal.cpp
// /*DetectGoal*/.cpp
// setting-> offset: +10, CMD_MAX=60

#include "DetectGoal.hpp"

#define LIGHT_WHITE  40
#define LIGHT_BLACK  0
#define TAIL_ANGLE_DRIVE   3
#define TIME_BAND   36

DetectGoal::DetectGoal(std::string name, Balancer* balancer) : Phase(name, balancer)
{
}

DetectGoal::DetectGoal(std::string name, Balancer* balancer,int setforwardl,int setforwardr,int maxspeedtime,int gyoffset) : Phase(name, balancer)
{
	forwardl=setforwardl;
	forwardr=setforwardr;
	mxsptime=maxspeedtime;
	gyrooffset=gyoffset;
}

void DetectGoal::initialize()
{
	balancer->change_offset(0);
	fp=fopen("detectgoallog.csv","w");
  if(fp==NULL){
    printf("failed\n");
    return;
  }
	fprintf(fp,"time,color\n");
	cnt=0;
	for(int i=0;i<100;i++){
		data[i]=0;
	}
}

void DetectGoal::calc_param(int time,int* forward,signed char* turn,int color){
	const float Kp = 1.1;
	const float Kd = 10.00;
	int diff_p = std::min(color,LIGHT_WHITE)-(LIGHT_WHITE+LIGHT_BLACK)/2;
	int diff_d = diff_p-(diff_prev);
	diff_prev=diff_p;
	int val=(Kp*diff_p+diff_d*Kd)/2;
	if (time == 0) val = (Kp * diff_p) / 2;
	(*turn)=(signed char)val;
	if(*turn<-50)
		*turn=-50;
	if(*turn>50)
		*turn=50;
}

void DetectGoal::calculate(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char* pwm_L, signed char* pwm_R, signed int* ang_T)
{
	int forward=forwardl+(forwardr-forwardl)*std::min(time,mxsptime)/mxsptime;
	signed char turn;
	calc_param(time,&forward,&turn,color);
	balancer->setCommand((signed char)forward, turn);
	balancer->update(gyro, motor_ang_r, motor_ang_l, volt);
	*pwm_L = balancer->getPwmLeft();
	*pwm_R = balancer->getPwmRight();
	*ang_T = TAIL_ANGLE_DRIVE;
	//fprintf(fp,"%d,%d\n",time,color);
}

bool DetectGoal::isFinished(int time, int32_t motor_ang_l, int32_t motor_ang_r, int color, int gyro, int sonic, int volt, signed char pwm_L, signed char pwm_R, signed int ang_T)
{
	int tim=time/4;
	data[tim%TIME_BAND]=color;
	if(time>=3000){
		int diff1=0;
		int diff2=0;
		for(int i=0;i<TIME_BAND/3;i++){
			diff1+=data[(tim-i+TIME_BAND)%TIME_BAND];
			diff2+=data[(tim+i+1)%TIME_BAND];
		}
		if(diff1-diff2>=(TIME_BAND/3)*10) return true;
	}
//	if(time>=10000)return true;
	return false;
}
