/*!*********************************************************
* @brief
*
* @param
*
* @retval
*
* @return
*
**********************************************************/
void lt_statnd(
void
)
{
S8 pwm_l;
S8 pwm_r;
int tail;
/* 倒立制御 */
balance_control(
(F32)0,
(F32)0,
(F32)ecrobot_get_gyro_sensor(PORT_GYRO),
GYRO_OFFSET,
(F32)nxt_motor_get_count(PORT_MOTOR_L),
(F32)nxt_motor_get_count(PORT_MOTOR_R),
(F32)ecrobot_get_battery_voltage(),
&pwm_l,
&pwm_r);
nxt_motor_set_speed(PORT_MOTOR_L, pwm_l, 1);
nxt_motor_set_speed(PORT_MOTOR_R, pwm_r, 1);
/* 尻尾制御 */
tail = nxt_motor_get_count(PORT_MOTOR_TAIL);
if (tail < -40) {
gkRobotStat.robotStat = ROBOT_STAT_TRACE;
//gkRobotStat.robotStat = ROBOT_STAT_GARAGE;
} else {
nxt_motor_set_speed(PORT_MOTOR_TAIL, (S8)-50, 1);
}
}
/*!*********************************************************
* @brief
*
* @param
*
* @retval
*
* @return
*
**********************************************************/
void lt_linetrace(
void
)
{
S8 pwm_l;
S8 pwm_r;
F32 forward;
F32 turn;
F32 u_turn;
U16 lightness;
lightness = ecrobot_get_light_sensor(PORT_LIGHT);
lt_calc_pid(lightness, &turn);
if (turn > 100) {
turn = 100;
} else if (turn < -100) {
turn = -100;
}
/* 絶対値をとる */
if (turn > 0) {
u_turn = turn;
} else {
u_turn = (-1) * turn;
}
/* 旋回量から前進量を調整する */
if (u_turn < 30) {
forward = 85;
} else if (u_turn < 50) {
forward = 85;
} else {
forward = 85;
}
//if (u_turn < 30) {
// forward = 100;
//} else if (u_turn < 50) {
// forward = 90;
//} else {
// forward = 85;
//}
balance_control(
(F32)forward,
turn,
(F32)ecrobot_get_gyro_sensor(PORT_GYRO),
GYRO_OFFSET,
(F32)nxt_motor_get_count(PORT_MOTOR_L),
(F32)nxt_motor_get_count(PORT_MOTOR_R),
(F32)ecrobot_get_battery_voltage(),
&pwm_l,
&pwm_r);
nxt_motor_set_speed(PORT_MOTOR_L, pwm_l, 1);
nxt_motor_set_speed(PORT_MOTOR_R, pwm_r, 1);
return;
}
// :3 Listen, here we will write the function that will decode only ONE order and will return some data, IF it's a sensors packet...
static void
decode_bro_input (const bro_fist_t * input_packet, bro_fist_t * output_packet, engines_t * motors )
{
U8 temp_port;
output_packet->port = input_packet->port;
output_packet->operation = input_packet->operation;
switch (input_packet->operation) {
case LIGHT_SENSOR:
decode_bro_port (input_packet->port, &temp_port);
output_packet->data = (float) ecrobot_get_light_sensor(temp_port);
break;
case TOUCH_SENSOR:
decode_bro_port (input_packet->port, &temp_port);
output_packet->data = (float) ecrobot_get_touch_sensor(temp_port);
break;
case SOUND_SENSOR:
decode_bro_port (input_packet->port, &temp_port);
output_packet->data = (float) ecrobot_get_sound_sensor(temp_port);
break;
case RADAR_SENSOR:
decode_bro_port (input_packet->port, &temp_port);
output_packet->data = (float) ecrobot_get_sonar_sensor(temp_port);
break;
case TACHO_COUNT:
decode_bro_port (input_packet->port, &temp_port);
output_packet->data = (float) nxt_motor_get_count(temp_port);
break;
case SET_POWER:
decode_bro_port (input_packet->port, &temp_port);
switch (temp_port) {
case NXT_PORT_A:
motors->first.speed_control_type = RAW_POWER;
motors->first.powers[0] = input_packet->data;
output_packet->data = (float) nxt_motor_get_count(NXT_PORT_A);
output_packet->timestamp = (float) systick_get_ms();
break;
case NXT_PORT_B:
motors->second.speed_control_type = RAW_POWER;
motors->second.powers[0] = input_packet->data;
break;
case NXT_PORT_C:
motors->third.speed_control_type = RAW_POWER;
motors->third.powers[0] = input_packet->data;
break;
};
break;
default:
//Nothing HERE
break;
};
}
void logSend(S8 data1, S8 data2, S16 adc1, S16 adc2, S16 adc3, S16 adc4, S32 datas32, S32 datas32_2) {
static U8 data_log_buffer[32];
//左辺は結局data_log_bufferの中身を表している。要するにただの代入
//32ビット分アクセスするためにdata_log_bufferの[1]から[3]へはアクセスしてない。
*((U32 *)(&data_log_buffer[0])) = (U32)systick_get_ms();
*(( S8 *)(&data_log_buffer[4])) = (S8)data1;
*(( S8 *)(&data_log_buffer[5])) = (S8)data2;
*((U16 *)(&data_log_buffer[6])) = (U16)ecrobot_get_light_sensor(NXT_PORT_S3); //light sensor value
*((S32 *)(&data_log_buffer[8])) = (S32)nxt_motor_get_count(0);
*((S32 *)(&data_log_buffer[12])) = (S32)nxt_motor_get_count(1);
*((S32 *)(&data_log_buffer[16])) = (S32)nxt_motor_get_count(2);
*((S16 *)(&data_log_buffer[20])) = (S16)adc1;
*((S16 *)(&data_log_buffer[22])) = (S16)adc2;
*((S16 *)(&data_log_buffer[24])) = (S16)adc3;
*((S16 *)(&data_log_buffer[26])) = (S16)adc4;
*((S32 *)(&data_log_buffer[28])) = (U32)datas32_2;
ecrobot_send_bt_packet(data_log_buffer, 32);
}
/*!*********************************************************
* @brief
*
* @param
*
* @retval
*
* @return
*
**********************************************************/
static U32 garage_stand(
F32 foward,
F32 turn,
F32 gyro_offset
)
{
S8 pwm_l;
S8 pwm_r;
U32 ret = GAGAGE_RET_FORE;
/* 倒立制御 */
balance_control(
(F32)foward,
(F32)turn,
(F32)ecrobot_get_gyro_sensor(PORT_GYRO),
gyro_offset,
(F32)nxt_motor_get_count(PORT_MOTOR_L),
(F32)nxt_motor_get_count(PORT_MOTOR_R),
(F32)ecrobot_get_battery_voltage(),
&pwm_l,
&pwm_r
);
nxt_motor_set_speed(PORT_MOTOR_L, pwm_l, 1);
nxt_motor_set_speed(PORT_MOTOR_R, pwm_r, 1);
if (pwm_l < 0 && pwm_r < 0) {
ret = GAGAGE_RET_REAR;
} else { /* nothing */ }
return ret;
}
//走行体状態設定関数
void RN_modesetting(){
switch (runner_mode){
//走行体初期状態
case (RN_MODE_INIT):
cmd_forward = 0;
cmd_turn = 0;
break;
//バランサー
case (RN_MODE_BALANCE):
balance_control(
(F32)cmd_forward,
(F32)cmd_turn,
(F32)ecrobot_get_gyro_sensor(NXT_PORT_S1),
(F32)gyro_offset,
(F32)nxt_motor_get_count(NXT_PORT_C),
(F32)nxt_motor_get_count(NXT_PORT_B),
(F32)ecrobot_get_battery_voltage(),
&pwm_l,
&pwm_r);
nxt_motor_set_speed(NXT_PORT_C, pwm_l, 1);
nxt_motor_set_speed(NXT_PORT_B, pwm_r, 1);
break;
//バランサー無し
case (RN_MODE_BALANCEOFF):
break;
default:
nxt_motor_set_speed(NXT_PORT_C, 0, 1);
nxt_motor_set_speed(NXT_PORT_B, 0, 1);
break;
}
}
/**
* 指定した角度だけ、ロボットを左回転させる
* 引数angleは、ロボットの回転角度
*/
extern BOOL BLNU_angle_turn_left(F32 angle) {
/** モータ回転コントロールを開始するときには、
* 最初の回転角度を取得する */
if (BLNU_motor_angle_control == false) {
BLNU_motor_angle_control = true;
start_l_motor_rev = nxt_motor_get_count(L_MOTOR_PORT); /* 左のモータの回転角度 */
start_r_motor_rev = nxt_motor_get_count(R_MOTOR_PORT); /* 右のモータの回転角度 */
F32 a = Robot_WIDTH; /* ロボットの横幅 mm */
F32 b = Wheel_DIAMETER; /* タイヤの直径 mm */
finish_motor_rev = 2* (a /b)*angle; /* angle度回転させるために必要な左右のモータの回転角度の合計値 */
}
/** 左に回転する */
BLNU_cmd_forward = 0;
BLNU_cmd_turn = -50;
/** 現在の左右のモータ回転角度の合計値が、目標値を超えたらtrueを返す */
if((nxt_motor_get_count(R_MOTOR_PORT) - start_r_motor_rev)
+ (start_l_motor_rev - nxt_motor_get_count(L_MOTOR_PORT)) >= finish_motor_rev) {
BLNU_motor_angle_control = false;
return true;
} else {
return false;
}
}
/*!*********************************************************
* @brief
*
* @param
*
* @retval
*
* @return
*
**********************************************************/
static void seesaw_stand(
F32 foward,
F32 turn,
F32 gyro_offset
)
{
S8 pwm_l;
S8 pwm_r;
/* 倒立制御 */
balance_control(
(F32)foward,
(F32)turn,
(F32)ecrobot_get_gyro_sensor(PORT_GYRO),
gyro_offset,
(F32)nxt_motor_get_count(PORT_MOTOR_L),
(F32)nxt_motor_get_count(PORT_MOTOR_R),
(F32)ecrobot_get_battery_voltage(),
&pwm_l,
&pwm_r
);
nxt_motor_set_speed(PORT_MOTOR_L, pwm_l, 1);
nxt_motor_set_speed(PORT_MOTOR_R, pwm_r, 1);
return;
}
void initialize_motors(engines_t * motors){
nxt_motor_set_count(NXT_PORT_A, 1000);
for(int i = 0; i < SAMP_NUM; i++){
motors->first.speeds[i] = 0;
motors->first.revolutions[i] = nxt_motor_get_count(NXT_PORT_A);
motors->first.times[i] = (long) ecrobot_get_systick_ms();
}
for(int j = 0; j < BUFFER_SIZE; j++){
motors->first.speed_ref[j] = 0;
motors->first.distance_ref[j] = 0;
}
nxt_motor_set_count(NXT_PORT_B, 1000);
for(int i = 0; i < SAMP_NUM; i++){
motors->second.speeds[i] = 0;
motors->second.revolutions[i] = nxt_motor_get_count(NXT_PORT_B);
motors->second.times[i] = (long) ecrobot_get_systick_ms();
}
for(int j = 0; j < BUFFER_SIZE; j++){
motors->first.speed_ref[j] = 0;
motors->first.distance_ref[j] = 0;
}
nxt_motor_set_count(NXT_PORT_C, 1000);
for(int i = 0; i < SAMP_NUM; i++){
motors->third.speeds[i] = 0;
motors->third.revolutions[i] = nxt_motor_get_count(NXT_PORT_C);
motors->third.times[i] = (long) ecrobot_get_systick_ms();
}
for(int j = 0; j < BUFFER_SIZE; j++){
motors->third.speed_ref[j] = 0;
motors->third.distance_ref[j] = 0;
}
}
/*!*********************************************************
* @brief
*
* @param
*
* @retval
*
* @return
*
**********************************************************/
static U32 garage_trace(
void
)
{
U32 ret = RET_REMAIN;
F32 forward;
F32 turn;
F32 u_turn;
U16 lightness;
static U32 cnt = 0;
lightness = ecrobot_get_light_sensor(PORT_LIGHT);
garage_calc_pid(lightness, &turn);
/* 旋回量をフィルタリング */
if (turn > 100) {
turn = 100;
} else if (turn < -100) {
turn = -100;
}
/* 旋回量の絶対値をとる */
if (turn > 0) {
u_turn = turn;
} else {
u_turn = (-1) * turn;
}
cnt++;
if (cnt > 250) {
/* 旋回量から前進量を調整する */
if (u_turn < 30) {
forward = 100;
} else if (u_turn < 50) {
forward = 90;
} else {
forward = 85;
}
/* 倒立振子制御 */
garage_stand(forward, turn, GYRO_OFFSET);
/* 灰色検知 */
if (lightness > 610) {
nxt_motor_set_count(PORT_MOTOR_TAIL, 0);
nxt_motor_set_speed(PORT_MOTOR_TAIL, (S8)10, 1);
gkGarageInfo.motor_r = nxt_motor_get_count(PORT_MOTOR_R);
gkGarageInfo.motor_l = nxt_motor_get_count(PORT_MOTOR_L);
ret = RET_FINISH;
} else { /* nothing */ }
} else {
forward = 50;
/* 倒立振子制御 */
garage_stand(forward, turn, GYRO_OFFSET);
}
return ret;
}
int distance()
{
int distance;
int revL;
int revR;
revL = nxt_motor_get_count(NXT_PORT_C);
revR = nxt_motor_get_count(NXT_PORT_B);
distance = fabs(CIRCUMFERENCE/360.0 * ((revL+revR)/2.0)); //段差突入時の距離を測定
return distance;
}
/**
* バランスコントロール
*/
void BLNU_balance_control(void) {
S8 pwm_l, pwm_r;
/* NXTway-GS C API balance control function (has to be invoked in 4msec period) */
balance_control((F32) BLNU_cmd_forward, (F32) BLNU_cmd_turn,
(F32) ecrobot_get_gyro_sensor(BLNU_PORT), (F32) BLNU_gyro_offset,
(F32) nxt_motor_get_count(L_MOTOR_PORT), (F32) nxt_motor_get_count(
R_MOTOR_PORT), (F32) ecrobot_get_battery_voltage(), &pwm_l,
&pwm_r);
nxt_motor_set_speed(L_MOTOR_PORT, pwm_l, 1);
nxt_motor_set_speed(R_MOTOR_PORT, pwm_r, 1);
}
//自己位置同定関数
void SelfLocation_SelfLocate(SelfLocation * this_SelfLocation)
{
/* 自己位置同定用変数 */
float d_theta_r; //1ステップ当たりの右車輪回転角度
float d_theta_l; //1ステップ当たりの左車輪回転角度
static float d_theta_r_t = 0; //前回のステップの右車輪回転角度
static float d_theta_l_t = 0; //前回のステップの左車輪回転角度
static double omega_r; //右車輪の回転角速度
static double omega_l; //左車輪の回転角速度
float v_r; //右車輪の回転速度
float v_l; //左車輪の回転速度
float v; //走行体の走行速度
float omega; //走行体の回転角速度
// static float x_r = 0; //車体のX座標
// static float y_r = 0; //車体のY座標
// static float theta_R = 0; //車体の角度
static float x_r_zero = 0; //X座標初期値
static float y_r_zero = 0; //Y座標初期値
static float theta_R_zero = 0; //車体角度初期値
d_theta_l = (float)nxt_motor_get_count(NXT_PORT_C) * M_PI / 180.0;
d_theta_r = (float)nxt_motor_get_count(NXT_PORT_B) * M_PI / 180.0;
omega_l = (d_theta_l - d_theta_l_t)/0.004;
omega_r = (d_theta_r - d_theta_r_t)/0.004;
v_l = (WHEEL_R * 0.1) * omega_l;
v_r = (WHEEL_R * 0.1) * omega_r;
v = (v_r + v_l) / 2.0;
omega = (v_r - v_l) / (MACHINE_W * 0.1);
d_theta_l_t = d_theta_l;
d_theta_r_t = d_theta_r;
this_SelfLocation -> direction += omega * 0.004 + theta_R_zero;
this_SelfLocation -> xCoo += v * cos(this_SelfLocation -> direction) * 0.004 + x_r_zero;
this_SelfLocation -> yCoo += v * sin(this_SelfLocation -> direction) * 0.004 + y_r_zero;
this_SelfLocation -> distance = fabs(CIRCUMFERENCE/360.0 * ((nxt_motor_get_count(NXT_PORT_C) + nxt_motor_get_count(NXT_PORT_B))/2.0));
}
PWMValues calcBalancePWMValue(PWMValues outputvalues)
{
balance_control(
(F32)getCmdForward(),
(F32)getCmdTurn(),
(F32)ecrobot_get_gyro_sensor(NXT_PORT_S1),
(F32)getGyroOffset(),
(F32)nxt_motor_get_count(NXT_PORT_C),
(F32)nxt_motor_get_count(NXT_PORT_B),
(F32)ecrobot_get_battery_voltage(),
&outputvalues.pwmL,
&outputvalues.pwmR);
return outputvalues;
}
float change_float_param(float param){
static int temp =0;
if((int)nxt_motor_get_count(NXT_PORT_C) - temp>0){
param=param+ADJUST_FLOAT_STEP;
}
else if((int)nxt_motor_get_count(NXT_PORT_C) - temp<0){
param=param-ADJUST_FLOAT_STEP;
}
temp=(int)nxt_motor_get_count(NXT_PORT_C);
systick_wait_ms(100);
param= round_n(param,2);
return param;
}
PWMValues PWMValGenerator_calBalancePWM(PWMValGenerator * this_PWMValGenerator,float forward,float turn) {
PWMValues pValues;
balance_control(
(F32)forward,
(F32)turn,
(F32)ecrobot_get_gyro_sensor(NXT_PORT_S1),
(F32)this_PWMValGenerator ->gyroOffset,
(F32)nxt_motor_get_count(NXT_PORT_C),
(F32)nxt_motor_get_count(NXT_PORT_B),
(F32)ecrobot_get_battery_voltage(),
&pValues.pwmL,
&pValues.pwmR);
return pValues;
}
//走行方法設定
void RN_modesetting()
{
switch (runner_mode){
case (RN_MODE_INIT):
cmd_forward = 0;
cmd_turn = 0;
break;
case (RN_MODE_CONTROL):
balance_control(
(F32)cmd_forward,
(F32)cmd_turn,
(F32)ecrobot_get_gyro_sensor(NXT_PORT_S1),
(F32)gyro_offset,
(F32)nxt_motor_get_count(NXT_PORT_C),
(F32)nxt_motor_get_count(NXT_PORT_B),
(F32)ecrobot_get_battery_voltage(),
&pwm_l,
&pwm_r);
nxt_motor_set_speed(NXT_PORT_C, pwm_l, 1);
nxt_motor_set_speed(NXT_PORT_B, pwm_r, 1);
break;
case (RN_MODE_CONTROL_2):
balance_control(
(F32)cmd_forward,
(F32)cmd_turn,
(F32)ecrobot_get_gyro_sensor(NXT_PORT_S1),
(F32)gyro_offset,
(F32)nxt_motor_get_count(NXT_PORT_C),
(F32)nxt_motor_get_count(NXT_PORT_B),
(F32)ecrobot_get_battery_voltage(),
&pwm_l,
&pwm_r);
nxt_motor_set_speed(NXT_PORT_C, pwm_l, 1);
nxt_motor_set_speed(NXT_PORT_B, pwm_r, 1);
break;
case (RN_MODE_STOP):
break;
default:
nxt_motor_set_speed(NXT_PORT_C, 0, 1);
nxt_motor_set_speed(NXT_PORT_B, 0, 1);
break;
}
}
//*****************************************************************************
// 関数名 : line_follow
// 引数 : speed, turn 走行速度、旋回速度
// 引数 : gyro_sensor ジャイロセンサー値
// 返り値 : 無し
// 概要 : ライントレース
//*****************************************************************************
void line_follow(int speed, int turn, int gyro_sensor)
{
signed char pwm_L, pwm_R; // 左右モータPWM出力
balance_control(
(float)speed, /* 前後進命令(+:前進, -:後進) */
(float)turn, /* 旋回命令(+:右旋回, -:左旋回) */
(float)gyro_sensor, /* ジャイロセンサ値 */
(float)GYRO_OFFSET, /* ジャイロセンサオフセット値 */
(float)nxt_motor_get_count(NXT_PORT_C), /* 左モータ回転角度[deg] */
(float)nxt_motor_get_count(NXT_PORT_B), /* 右モータ回転角度[deg] */
(float)ecrobot_get_battery_voltage(), /* バッテリ電圧[mV] */
&pwm_L, /* 左モータPWM出力値 */
&pwm_R /* 右モータPWM出力値 */
);
nxt_motor_set_speed(NXT_PORT_C, pwm_L, 1); /* 左モータPWM出力セット(-100〜100) */
nxt_motor_set_speed(NXT_PORT_B, pwm_R, 1); /* 右モータPWM出力セット(-100〜100) */
}
/*!*********************************************************
* @brief
*
* @param
*
* @retval
*
* @return
*
**********************************************************/
void lt_pol_triger(
void
)
{
U8 touch;
U32 bt = 0;
char buf[128];
U32 size;
S8 pwm_l;
S8 pwm_r;
/* タッチセンサ */
touch = ecrobot_get_touch_sensor(PORT_TOUCH);
/* Bluetooth */
size = ecrobot_read_bt(buf, 0, 128);
if (size > 0) {
if (buf[0] == 's') {
ecrobot_send_bt("start", 0, 5);
bt = 1;
} else {
ecrobot_send_bt("unknown", 0, 7);
}
} else { /* nothing */ }
if (touch == 1 || bt == 1) {
/* 倒立制御 */
balance_control(
(F32)0,
(F32)0,
(F32)ecrobot_get_gyro_sensor(PORT_GYRO),
GYRO_OFFSET,
(F32)nxt_motor_get_count(PORT_MOTOR_L),
(F32)nxt_motor_get_count(PORT_MOTOR_R),
(F32)ecrobot_get_battery_voltage(),
&pwm_l,
&pwm_r);
nxt_motor_set_speed(PORT_MOTOR_L, pwm_l, 1);
nxt_motor_set_speed(PORT_MOTOR_R, pwm_r, 1);
nxt_motor_set_count(PORT_MOTOR_TAIL, (int)0);
gkRobotStat.robotStat = ROBOT_STAT_STAND;
} else { /* NOTHING */ }
}
int change_int_param(int param){
static int temp =0;
//�O��擾������]�p�Ƃ̍���p���Ēl�𐧌�
if((int)nxt_motor_get_count(NXT_PORT_C) - temp>0){
param=param+ADJUST_INT_STEP;
}
else if((int)nxt_motor_get_count(NXT_PORT_C) - temp<0){
param=param-ADJUST_INT_STEP;
}
temp=(int)nxt_motor_get_count(NXT_PORT_C);
systick_wait_ms(SET_PARAM_SPEED); /* 100msec�E�F�C�g */
return param;
}
//bluetoothログ送信関数
void logSend(S8 data1, S8 data2, S16 adc1, S16 adc2, S16 adc3, S16 adc4){
U8 data_log_buffer[32];
*((U32 *)(&data_log_buffer[0])) = (U32)systick_get_ms();
*(( S8 *)(&data_log_buffer[4])) = (S8)data1;
*(( S8 *)(&data_log_buffer[5])) = (S8)data2;
*((U16 *)(&data_log_buffer[6])) = (U16)ecrobot_get_light_sensor(NXT_PORT_S3);
*((S32 *)(&data_log_buffer[8])) = (S32)nxt_motor_get_count(0);
*((S32 *)(&data_log_buffer[12])) = (S32)nxt_motor_get_count(1);
*((S32 *)(&data_log_buffer[16])) = (S32)nxt_motor_get_count(2);
*((S16 *)(&data_log_buffer[20])) = (S16)adc1;
*((S16 *)(&data_log_buffer[22])) = (S16)adc2;
*((S16 *)(&data_log_buffer[24])) = (S16)adc3;
*((S16 *)(&data_log_buffer[26])) = (S16)adc4;
*((S32 *)(&data_log_buffer[28])) = (S32)ecrobot_get_sonar_sensor(NXT_PORT_S2);
ecrobot_send_bt_packet(data_log_buffer, 32);
}
// Funcion para mover el motor izq. o der. una distancia 'y' a una velocidad 'z'
void mover_motor(U32 motor, int distancia, int velocidad)
{
int grados = (360*distancia)/18;
if(velocidad < 0){
grados = grados*(-1);
}
int ref = nxt_motor_get_count(motor) + grados;
nxt_motor_set_speed(motor, velocidad, 1);
if(velocidad < 0){
while(nxt_motor_get_count(motor) > ref);
}
else{
while(nxt_motor_get_count(motor) < ref);
}
nxt_motor_set_speed(motor, 0, 1);
}
void updateMotors(engines_t * motors){
motor_t *motor;
motor_t *otherMotor;
for(int m = 0; m < 2; m++){
switch (m){
case 0: motor = &(motors->first); otherMotor = &(motors->second); break;
case 1: motor = &(motors->second); otherMotor = &(motors->first); break;
}
for (int i = SAMP_NUM; i > 0; i--){
motor->revolutions[i] = motor->revolutions[i-1];
motor->speeds[i] = motor->speeds[i-1];
}
motor->revolutions[0] = nxt_motor_get_count(motor->port);
motor->times[0] = ecrobot_get_systick_ms();
motor->speeds[0] = evaluate_speed(motor, motor->speeds[1]);
if(motor->speed_control_type == NOT_USING)
continue;
if(motor->speed_control_type == PID_CONTROLLED){
double distance = (((motor->revolutions[0] + otherMotor->revolutions[0])/2.0) * PI/180) * 0.028;
if(abs(distance) >= motor->distance_ref[0]){
for (int i = 0; i < BUFFER_SIZE - 1; i++){
motor->speed_ref[i] = motor->speed_ref[i+1];
motor->distance_ref[i] = motor->distance_ref[i+1];
otherMotor->speed_ref[i] = otherMotor->speed_ref[i+1];
otherMotor->distance_ref[i] = otherMotor->distance_ref[i+1];
}
motor->speed_ref[BUFFER_SIZE-1] = 0;
motor->distance_ref[BUFFER_SIZE-1] = 0;
otherMotor->speed_ref[BUFFER_SIZE-1] = 0;
otherMotor->distance_ref[BUFFER_SIZE-1] = 0;
}
if(distance < motor->distance_ref[0]){
double error = motor->speed_ref[0] - motor->speeds[0];
double rotationError = rotationController(motors);
if(m == 0) {
error = error - rotationError/2;
motor->powers[0] = controller(error);
}
if(m == 1) {
error = error + rotationError/2;
motor->powers[0] = controller2(error);
}
}
}
nxt_motor_set_speed(motor->port, motor->powers[0], 1);
}
}
/*!*********************************************************
* @brief
*
* @param
*
* @retval
*
* @return
*
**********************************************************/
static U32 garage_tail_down(
void
)
{
U32 ret = RET_REMAIN;
U32 garage_ret;
int tail;
int motor_r;
int motor_l;
F32 forward;
static U32 cnt = 0;
motor_r = nxt_motor_get_count(PORT_MOTOR_R);
motor_l = nxt_motor_get_count(PORT_MOTOR_L);
if (motor_r - gkGarageInfo.motor_r < 100 || motor_l - gkGarageInfo.motor_l < 100) {
forward = 1;
} else {
forward = -1;
}
garage_ret = garage_stand(forward, 0, GYRO_OFFSET);
/* 尻尾制御 */
tail = nxt_motor_get_count(PORT_MOTOR_TAIL);
if (tail > GARAGE_COCK_START_ANGLE) {
nxt_motor_set_speed(PORT_MOTOR_TAIL, (S8)0, 1);
if ((motor_r - gkGarageInfo.motor_r) > 100 || (motor_l - gkGarageInfo.motor_l) > 100) {
cnt++;
if (cnt > 50) {
if (garage_ret == GAGAGE_RET_REAR) {
//garage_stand((F32)0, (F32)0, (GYRO_OFFSET - 40));
nxt_motor_set_speed(PORT_MOTOR_R, (S8)0, 1);
nxt_motor_set_speed(PORT_MOTOR_L, (S8)0, 1);
ecrobot_set_light_sensor_inactive(PORT_LIGHT);
ret = RET_FINISH;
} else { /* nothing */ }
} else { /* nothing */ }
}
} else { /* nothing */ }
return ret;
}
//衝撃検知
void shock(void){
if(min_vol>ecrobot_get_battery_voltage())
min_vol=ecrobot_get_battery_voltage();
if((ecrobot_get_gyro_sensor(NXT_PORT_S1) <= gyro_offset-PM_GYRO ||
ecrobot_get_gyro_sensor(NXT_PORT_S1) >= gyro_offset+PM_GYRO) &&
min_vol <= battery_value-DOWN_BATTERY)
{
ecrobot_sound_tone(880,512,30);
if(runner_mode == RN_MODE_CONTROL){
revL = nxt_motor_get_count(NXT_PORT_C);
revR = nxt_motor_get_count(NXT_PORT_B);
distance_before = fabs(CIRCUMFERENCE/360.0 * ((revL+revR)/2.0));
setting_mode = RN_SLOW_RUN;
runner_mode = RN_MODE_CONTROL_2;
gyro_offset += 10;
min_vol = battery_value;
}
}
}
/*!*********************************************************
* @brief
*
* @param
*
* @retval
*
* @return
*
*********************************************************/
static U32 seesaw_forward(
void
)
{
U32 ret = RET_REMAIN;
U16 lightness;
F32 turn;
static int motor_r = 0;
static int motor_l = 0;
int cur_motor_r;
int cur_motor_l;
lightness = ecrobot_get_light_sensor(PORT_LIGHT);
turn = (0.2) * (530 - lightness);
seesaw_stand((F32)15, (F32)turn, GYRO_OFFSET + 2);
//seesaw_stand((F32)1, (F32)0, GYRO_OFFSET);
if (motor_r == 0) {
motor_r = nxt_motor_get_count(PORT_MOTOR_R);
motor_l = nxt_motor_get_count(PORT_MOTOR_L);
nxt_motor_set_count(PORT_MOTOR_TAIL, 0);
} else {
///* 尻尾制御 */
//tail = nxt_motor_get_count(PORT_MOTOR_TAIL);
//if (tail < SEESAW_TAIL_ANGLE_BLAKE) {
// nxt_motor_set_speed(PORT_MOTOR_TAIL, 20, 1);
//} else {
// nxt_motor_set_speed(PORT_MOTOR_TAIL, 0, 1);
//}
cur_motor_r = nxt_motor_get_count(PORT_MOTOR_R);
cur_motor_l = nxt_motor_get_count(PORT_MOTOR_L);
if (cur_motor_r - motor_r > 900 && cur_motor_l - motor_l > 900) {
ret = RET_FINISH;
} else { /* nothing */ }
}
return ret;
}
//*****************************************************************************
// 関数名 : tail_control
// 引数 : angle (モータ目標角度[度])
// 返り値 : 無し
// 概要 : 走行体完全停止用モータの角度制御
//*****************************************************************************
static void tail_control(signed int angle)
{
float pwm = (float)(angle - nxt_motor_get_count(NXT_PORT_A))*P_GAIN; /* 比例制御 */
/* PWM出力飽和処理 */
if (pwm > PWM_ABS_MAX) {
pwm = PWM_ABS_MAX;
}
else if (pwm < -PWM_ABS_MAX) {
pwm = -PWM_ABS_MAX;
}
nxt_motor_set_speed(NXT_PORT_A, (signed char)pwm, 1);
}
void self_location()
{
d_theta_l = (float)nxt_motor_get_count(NXT_PORT_C) * M_PI / 180.0;
d_theta_r = (float)nxt_motor_get_count(NXT_PORT_B) * M_PI / 180.0;
omega_l = (d_theta_l - d_theta_l_t)/0.004;
omega_r = (d_theta_r - d_theta_r_t)/0.004;
velocity_l = (WHEEL_R * 0.1) * omega_l;
velocity_r = (WHEEL_R * 0.1) * omega_r;
velocity = (velocity_r + velocity_l) / 2.0;
omega = (velocity_r - velocity_l) / (MACHINE_W * 0.1);
d_theta_l_t = d_theta_l;
d_theta_r_t = d_theta_r;
theta += omega * 0.004 + THETA_0;
position_x += velocity * cos(theta) * 0.004 + POSITION_X0;
position_y += velocity * sin(theta) * 0.004 + POSITION_Y0;
}
//自己位置同定関数
void self_location()
{
d_theta_l = (float)nxt_motor_get_count(NXT_PORT_C) * PI / 180.0;
d_theta_r = (float)nxt_motor_get_count(NXT_PORT_B) * PI / 180.0;
omega_l = (d_theta_l - d_theta_l_t)/0.004;
omega_r = (d_theta_r - d_theta_r_t)/0.004;
v_l = (WHEEL_R * 0.1) * omega_l;
v_r = (WHEEL_R * 0.1) * omega_r;
v = (v_r + v_l) / 2.0;
omega = (v_r - v_l) / (MACHINE_W * 0.1);
d_theta_l_t = d_theta_l;
d_theta_r_t = d_theta_r;
theta_R += omega * 0.004 + theta_R_zero;
x_r += v * cos(theta_R) * 0.004 + x_r_zero;
y_r += v * sin(theta_R) * 0.004 + y_r_zero;
}
float change_float_param(float param){
static int temp =0;
//�O��擾������]�p�Ƃ̍���p���Ēl�𐧌�
if((int)nxt_motor_get_count(NXT_PORT_C) - temp>0){
param=param+ADJUST_FLOAT_STEP/*0.001*/;
// param=(param+0.1)/*(float)ADJUST_FLOAT_STEP*/;
}
else if((int)nxt_motor_get_count(NXT_PORT_C) - temp<0){
param=param-ADJUST_FLOAT_STEP/*0.001*/;
//param=param-0.1;/*(float)ADJUST_FLOAT_STEP);*/
}
temp=(int)nxt_motor_get_count(NXT_PORT_C);
systick_wait_ms(SET_PARAM_SPEED); // 100msec�E�F�C�g
return param;
}
