/*!*********************************************************
* @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
*
**********************************************************/
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;
}
//*****************************************************************************
// FUNCTION : ecrobot_device_terminate
// ARGUMENT : none
// RETURN : none
// DESCRIPTION : ECRobot device term hook function
//*****************************************************************************
void ecrobot_device_terminate(void)
{
nxt_motor_set_speed(PORT_MOTOR_L, 0, 1);
nxt_motor_set_speed(PORT_MOTOR_R, 0, 1);
ecrobot_term_sonar_sensor(PORT_SONAR);
ecrobot_term_bt_connection();
}
/*!*********************************************************
* @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 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;
}
}
/*!*********************************************************
* @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;
}
void calcPWMValues()
{
PWMValues Runningvalues;
Runningvalues.pwmL = 0;
Runningvalues.pwmR = 0;
switch(generatormode){
case (RN_MODE_INIT):
break;
case (RN_MODE_BALANCE):
Runningvalues = calcBalancePWMValue(Runningvalues);
break;
case (RN_MODE_TAIL):
Runningvalues = calcTailPWMValue(Runningvalues);
break;
case (RN_MODE_STOP):
Runningvalues.pwmL = 0;
Runningvalues.pwmR = 0;
break;
default:
break;
}
nxt_motor_set_speed(NXT_PORT_C, Runningvalues.pwmL, 1);
nxt_motor_set_speed(NXT_PORT_B, Runningvalues.pwmR, 1);
}
//*****************************************************************************
// 関数名 : ecrobot_device_terminate
// 引数 : なし
// 戻り値 : なし
// 概要 : ECROBOTデバイス終了処理フック関数
//*****************************************************************************
void ecrobot_device_terminate()
{
nxt_motor_set_speed(NXT_PORT_C,0,0);
nxt_motor_set_speed(NXT_PORT_B,0,0);
nxt_motor_set_speed(NXT_PORT_A,0,0);
ecrobot_set_light_sensor_inactive(NXT_PORT_S3); /* 光センサ赤色LEDをOFF */
ecrobot_term_sonar_sensor(NXT_PORT_S2); /* 超音波センサ(I2C通信)を終了 */
ecrobot_term_bt_connection(); /* Bluetooth通信を終了 */
}
/**
* バランスコントロール
*/
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);
}
inline std_return RTE_SchussMotor_SetOutputValue_OSPort_Out(uint32_t* degree)
{
#define CORRECTION 11
#ifdef RTE_SchussMotor_SetOutputValue_OSPort_Out_ENGINE
nxt_motor_set_count(RTE_SchussMotor_SetOutputValue_OSPort_Out_PORT, 0);
nxt_motor_set_speed(RTE_SchussMotor_SetOutputValue_OSPort_Out_PORT, 35, 1);
while ( nxt_motor_get_count(RTE_SchussMotor_SetOutputValue_OSPort_Out_PORT) < (*degree - CORRECTION) )
{
;
}
nxt_motor_set_speed(RTE_SchussMotor_SetOutputValue_OSPort_Out_PORT, 0, 1);
#endif
return 0;
}
//ON-OFF制御ライントレース関数
void RA_linetrace(int forward_speed, int turn_speed) {
cmd_forward = forward_speed;
int light_value = 0;
light_value = online();
if (TRUE != light_value) {
cmd_turn = turn_speed;
} else {
cmd_turn = (-1)*turn_speed;
}
nxt_motor_set_speed(NXT_PORT_C, forward_speed - cmd_turn/2, 1);
nxt_motor_set_speed(NXT_PORT_B, forward_speed + cmd_turn/2, 1);
}
void motorTest_refined_model__proces_th_entrypoint_impl(int * cc5_portNb, int * cc6_brake, int * cc7_speed)
{
StatusType motorTest_refined_model__proces_th_entrypoint_impl_proces_th_runtime_call_ret;
motorTest_refined_model__proces_th_behaviorIdentifier_enum whichPortActivated = motorTest_refined_model__proces_th_behaviorIdentifier_enum_default_behavior;
while(1)
{
switch(proces_th_entrypoint_impl_current_state)
{
case motorTest_refined_model__proces_th_entrypoint_impl_BA_entrypoint_init_state:
// Transition id: which_behavior_default_mode
if(1) // no execution condition
{
proces_th_entrypoint_impl_current_state = motorTest_refined_model__proces_th_entrypoint_impl_BA_entrypoint_exec_state;
whichPortActivated = motorTest_refined_model__proces_th_behaviorIdentifier_enum_default_behavior;
break;
}
case motorTest_refined_model__proces_th_entrypoint_impl_BA_entrypoint_wait_dispatch_state:
// Transition id: dispatch_transition
if(1) // no execution condition
{
proces_th_entrypoint_impl_current_state = motorTest_refined_model__proces_th_entrypoint_impl_BA_entrypoint_exec_state;
TerminateTask ();
break;
}
case motorTest_refined_model__proces_th_entrypoint_impl_BA_entrypoint_exec_state:
// Transition id: call1 -- Priority 0
if(whichPortActivated == motorTest_refined_model__proces_th_behaviorIdentifier_enum_default_behavior)
{
proces_th_entrypoint_impl_current_state = motorTest_refined_model__proces_th_entrypoint_impl_BA_entrypoint_wait_dispatch_state;
nxt_motor_set_speed (*cc5_portNb, *cc7_speed, *cc6_brake);
break;
}
}
}
}
/*!*********************************************************
* @brief
*
* @param
*
* @retval
*
* @return
*
**********************************************************/
static U32 garage_stop(
void
)
{
U32 ret = RET_REMAIN;
int tail;
/* 尻尾制御 */
tail = nxt_motor_get_count(PORT_MOTOR_TAIL);
if (tail > GARAGE_COCK_START_ANGLE) {
nxt_motor_set_speed(PORT_MOTOR_TAIL, (S8)-5, 1);
} else {
nxt_motor_set_speed(PORT_MOTOR_TAIL, (S8)5, 1);
}
return ret;
}
//*****************************************************************************
// 関数名 : line_follow2
// 引数 : Black (黒のセンサ値)
// 引数 : white (白のセンサ値)
// 返り値 : 無し
// 概要 : バランサーを使用しないライントレース
//*****************************************************************************
static void line_follow2(int speed, int black, int white)
{
int pwm_L, pwm_R, turn2;
turn2 = KP * (ecrobot_get_light_sensor(NXT_PORT_S3) - TH2(black, white));
if (turn2 > 50) turn = 50;
if (turn2 < -50) turn = -50;
pwm_L = speed - turn2;
pwm_R = speed + turn2;
if (pwm_L > 100) pwm_L = 100;
if (pwm_L < -100) pwm_L = -100;
if (pwm_R > 100) pwm_R = 100;
if (pwm_R < -100) pwm_R = -100;
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) */
//xsprintf(tx_buf,"lf2:turn2=%d,%d\n",turn2,ecrobot_get_light_sensor(NXT_PORT_S3));
//ecrobot_send_bt(tx_buf,0,strlen(tx_buf));
}
//*****************************************************************************
// 関数名 : 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
*
**********************************************************/
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;
}
/*!*********************************************************
* @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 */ }
}
// 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;
}
//*****************************************************************************
// 関数名 : line_follow3
// 引数 : Black (黒のセンサ値)
// 引数 : white (白のセンサ値)
// 返り値 : 無し
// 概要 : バランサーを使用しないライントレース
//*****************************************************************************
static void line_follow3(int speed, int black, int white)
{
int pwm_L, pwm_R;
int turn2 = 10;
if(ecrobot_get_light_sensor(NXT_PORT_S3) > TH2(black, white)){
pwm_R = speed + turn2;
pwm_L = speed;
}else{
pwm_R = speed;
pwm_L = speed + turn2;
}
if (pwm_L > 100) pwm_L = 100;
if (pwm_L < -100) pwm_L = -100;
if (pwm_R > 100) pwm_R = 100;
if (pwm_R < -100) pwm_R = -100;
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) */
//xsprintf(tx_buf,"lf2:turn2=%d,%d\n",turn2,ecrobot_get_light_sensor(NXT_PORT_S3));
//ecrobot_send_bt(tx_buf,0,strlen(tx_buf));
}
//PID制御ライントレース関数（バランサーOFF用）
void RA_linetrace_PID_balanceoff(int forward_speed){
RA_speed(forward_speed,1); //速度を段階的に変化
if(forward_speed > 0)
hensa = (float)LOOKUP_GRAY_VALUE - (float)ecrobot_get_light_sensor(NXT_PORT_S3);
else
hensa = (float)ecrobot_get_light_sensor(NXT_PORT_S3) - (float)LOOKUP_GRAY_VALUE;
i_hensa = i_hensa+(hensa*0.0005);
d_hensa = (hensa - bf_hensa)/0.0005;
bf_hensa = hensa;
cmd_turn = -(Kp * hensa + Ki * i_hensa + Kd * d_hensa);
if (-100 > cmd_turn) {
cmd_turn = -100;
} else if (100 < cmd_turn) {
cmd_turn = 100;
}
nxt_motor_set_speed(NXT_PORT_C, forward_speed + cmd_turn/2, 1);
nxt_motor_set_speed(NXT_PORT_B, forward_speed - cmd_turn/2, 1);
}
/* nxtOSEK hooks */
void ecrobot_device_initialize(void)
{
nxt_motor_set_speed(PORT_MOTOR_L, 0, 1);
nxt_motor_set_speed(PORT_MOTOR_R, 0, 1);
balance_init();
nxt_motor_set_count(PORT_MOTOR_L, 0);
nxt_motor_set_count(PORT_MOTOR_R, 0);
ecrobot_init_sonar_sensor(PORT_SONAR);
ecrobot_set_light_sensor_active(PORT_LIGHT);
/* ���O */
if (ecrobot_get_bt_status() == BT_NO_INIT) {
ecrobot_set_bt_device_name("ETROBOLT255");
} else { /* NOTHING */ }
ecrobot_init_bt_slave("LEJOS_OSEK");
lt_ini();
lookup_ini();
seesaw_ini();
garage_ini();
}
//*****************************************************************************
// 関数名 : 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);
}
// Funcion para mover dos motores x la distancia y a la velocidad z
void mover_motores(U32 motor1, U32 motor2, int dist1, int dist2, int vel1, int vel2)
{
int llega1 = 0; // flag para determinar el final del movimiento del motor 1
int llega2 = 0; // flag para determinar el final del movimiento del motor 1
int dir1 = 1; //1 -> adelante, -1 -> atras, 0 -> parado
int dir2 = 1; //1 -> adelante, -1 -> atras, 0 -> parado
// Obtenemos los grados que tienen que girar las ruedas
int grados1 = (360*dist1)/188;
int grados2 = (360*dist2)/188;
// Esta conversion permite especificar velocidades negativas y positivas
if(vel1 < 0){
grados1 = -grados1;
dir1 = -dir1;
}
if(vel2 < 0){
grados2 = -grados2;
dir2 = -dir2;
}
// Calculamos angulos de referencia
int ref1 = nxt_motor_get_count(motor1) + grados1;
int ref2 = nxt_motor_get_count(motor2) + grados2;
// Iniciamos los motores
nxt_motor_set_speed(motor1, vel1, 1);
nxt_motor_set_speed(motor2, vel2, 1);
// Comprobamos si hemos llegado al angulo de referencia y si es asi
// paramos los motores
while((llega1 != 1)||(llega2 != 1)){
if(dir1 < 0){
if(nxt_motor_get_count(motor1) <= ref1){
llega1 = 1;
nxt_motor_set_speed(motor1,0,1);
}
}
else{
if(nxt_motor_get_count(motor1) >= ref1){
llega1 = 1;
nxt_motor_set_speed(motor1,0,1);
}
}
if(dir2 < 0){
if(nxt_motor_get_count(motor2) <= ref2){
llega2 = 1;
nxt_motor_set_speed(motor2,0,1);
}
}
else{
if(nxt_motor_get_count(motor2) >= ref2){
llega2 = 1;
nxt_motor_set_speed(motor2,0,1);
}
}
}
}
S32
MotorPID(S32 target, U8 motor)
{
static S32 integrale[] = {0, 0, 0, 0};
static S32 lastError[] = {0, 0, 0, 0};
static S32 lastTarget[] = {0, 0, 0, 0};
if(lastTarget[motor] != target)
{
integrale[motor] = 0;
lastError[motor] = 0;
lastTarget[motor] = target;
}
register S32 current = nxt_motor_get_count(motor);
register S32 getSpeed = PID(target, current, &integrale[motor], &lastError[motor]);
nxt_motor_set_speed(motor, getSpeed, 0);
return getSpeed;
}
//走行方法設定
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;
}
}
void updateMotors(engines_t * motors){
motor_t *motor;
motor_t *otherMotor;
motor = &(motors->first);
otherMotor = &(motors->second);
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 error = motor->speed_ref[0] - motor->speeds[0];
motor->powers[0] = controller(error);
}
nxt_motor_set_speed(motor->port, motor->powers[0], 1);
}
void stop() {
nxt_motor_set_speed(NXT_PORT_A, 0, 1);
nxt_motor_set_speed(NXT_PORT_B, 0, 1);
}
void forward_expanded(int speed, int dist)
{
nxt_motor_set_speed(NXT_PORT_A, speed, 0);
nxt_motor_set_speed(NXT_PORT_B, speed, 0);
}
