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;
}
}
int Calibration_calibration(Calibration *this_Calibration){
static U32 avg_cnt = 0;
static U32 cal_start_time;
//gyro_offset
while (1){
if(PushButton_detect_push_button(&pushButton) == TRUE)break;
}
ecrobot_sound_tone(880, 512, 10);
/* ジャイロセンサの値を計算するための開始時間をセットする */
cal_start_time = ecrobot_get_systick_ms();
while((ecrobot_get_systick_ms() - cal_start_time) < 1000U){
/* ジャイロセンサの設定をする */
this_Calibration->gyro += InclinationEncoder_get_inclination(&inclinationEncoder);
avg_cnt++;
}
this_Calibration->gyro /= avg_cnt;
ecrobot_sound_tone(440U, 500U, 10);
systick_wait_ms(500);
//black
while(1){
if(PushButton_detect_push_button(&pushButton) == TRUE){
ecrobot_sound_tone(906, 512, 10);
BrightnessEncoder_set_black(&brightnessEncoder,BrightnessEncoder_get_brightness(&brightnessEncoder));
systick_wait_ms(500);
break;
}
}
//white
while(1){
if(PushButton_detect_push_button(&pushButton) == TRUE){
ecrobot_sound_tone(906, 512, 10);
BrightnessEncoder_set_white(&brightnessEncoder,BrightnessEncoder_get_brightness(&brightnessEncoder));
systick_wait_ms(500);
break;
}
}
InclinationEncoder_set_gyro_offset(&inclinationEncoder ,this_Calibration->gyro);
InclinationEncoder_set_initial_gyro_offset(&inclinationEncoder ,this_Calibration->gyro);
while(1){
if(PushButton_detect_push_button(&pushButton) == TRUE){
systick_wait_ms(500);
break;
}
}
//キャリブレーションが終了したら1を返す
return 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);
}
}
void RN_set_gyro_start()
{
/* ジャイロセンサの設定を始める */
if (ecrobot_get_touch_sensor(NXT_PORT_S4) == TRUE) {
ecrobot_sound_tone(880, 512, 30);
/* ジャイロセンサの値を計算するための開始時間をセットする */
cal_start_time = ecrobot_get_systick_ms();
setting_mode = RN_SETTINGMODE_GYRO;
}
}
void RN_set_gyro()
{
/* ジャイロセンサの設定をする */
gyro_offset += (U32)ecrobot_get_gyro_sensor(NXT_PORT_S1);
avg_cnt++;
/* 1秒経過したら、ジャイロセンサのオフセット値の平均値を計算し、次の状態に遷移する。 */
if ((ecrobot_get_systick_ms() - cal_start_time) >= 1000U) {
gyro_offset /= avg_cnt;
ecrobot_sound_tone(440U, 500U, 30U);
setting_mode = RN_SETTINGMODE_GYRO_END;
}
}
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);
}
// :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 (bro_fist_t * input_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);
input_packet->data = (float) ecrobot_get_light_sensor(temp_port);
break;
case TOUCH_SENSOR:
decode_bro_port (input_packet->port, &temp_port);
input_packet->data = (float) ecrobot_get_touch_sensor(temp_port);
break;
case SOUND_SENSOR:
decode_bro_port (input_packet->port, &temp_port);
input_packet->data = (float) ecrobot_get_sound_sensor(temp_port);
break;
case RADAR_SENSOR:
decode_bro_port (input_packet->port, &temp_port);
input_packet->data = (float) ecrobot_get_sonar_sensor(temp_port);
break;
case TACHO_COUNT:
decode_bro_port (input_packet->port, &temp_port);
input_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;
input_packet->data = (float) nxt_motor_get_count(temp_port);
input_packet->time = (long) ecrobot_get_systick_ms();
break;
case NXT_PORT_B:
motors->second.speed_control_type = RAW_POWER;
motors->second.powers[0] = input_packet->data;
input_packet->data = (float) nxt_motor_get_count(temp_port);
input_packet->time = (long) ecrobot_get_systick_ms();
break;
case NXT_PORT_C:
motors->third.speed_control_type = RAW_POWER;
motors->third.powers[0] = input_packet->data;
input_packet->data = (float) nxt_motor_get_count(temp_port);
input_packet->time = (long) ecrobot_get_systick_ms();
break;
};
break;
case SET_SPEED:
motors->first.speed_control_type = PID_CONTROLLED;
motors->first.speed_ref = input_packet->data;
break;
default:
//Nothing HERE
break;
};
}
//*****************************************************************************
// TASK : tsk0
// DESCRIPTION : 4msec periodical Task and controls NXTway-GS
// INIT_MODE
// ��
// CAL_MODE
// ��
// CONTROL_MODE
//*****************************************************************************
void tsk0(VP_INT exinf)
{
static U32 gyro_offset; /* gyro sensor offset value */
static U32 avg_cnt; /* average count to calc gyro offset */
static U32 cal_start_time; /* calibration start time */
static U8 bt_receive_buf[BT_RCV_BUF_SIZE]; /* Bluetooth receive buffer(32bytes) */
SINT i;
S8 cmd_forward, cmd_turn;
S8 pwm_l, pwm_r;
switch(nxtway_gs_mode){
case(INIT_MODE):
gyro_offset = 0;
avg_cnt = 0;
for (i = 0; i < BT_RCV_BUF_SIZE; i++){
bt_receive_buf[i] = 0;
}
balance_init(); /* NXTway-GS C API initialize function */
nxt_motor_set_count(PORT_MOTOR_L, 0); /* reset left motor count */
nxt_motor_set_count(PORT_MOTOR_R, 0); /* reset right motor count */
cal_start_time = ecrobot_get_systick_ms();
nxtway_gs_mode = CAL_MODE;
break;
case(CAL_MODE):
gyro_offset += (U32)ecrobot_get_gyro_sensor(PORT_GYRO);
avg_cnt++;
if ((ecrobot_get_systick_ms() - cal_start_time) >= 1000U) {
/* 1000msec later, start balancing */
gyro_offset /= avg_cnt;
ecrobot_sound_tone(440U, 500U, 30U); /* beep a tone */
nxtway_gs_mode = CONTROL_MODE;
}
break;
case(CONTROL_MODE):
(void)ecrobot_read_bt_packet(bt_receive_buf, BT_RCV_BUF_SIZE);
/*
* R/C command from NXT GamePad
* buf[0]: -100(forward max.) to 100(backward max.)
* buf[1]: -100(turn left max.) to 100(turn right max.)
*/
cmd_forward = -(S8)bt_receive_buf[0]; /* reverse the direction */
cmd_turn = (S8)bt_receive_buf[1];
if (obstacle_flag == 1){
/* make NXTway-GS move backward to avoid obstacle */
cmd_forward = -100;
cmd_turn = 0;
}
/* NXTway-GS C API balance control function (has to be invoked in 4msec period) */
balance_control(
(F32)cmd_forward,
(F32)cmd_turn,
(F32)ecrobot_get_gyro_sensor(PORT_GYRO),
(F32)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);
ecrobot_bt_data_logger(cmd_forward, cmd_turn); /* Bluetooth data logger */
break;
default:
/* unexpected mode */
nxt_motor_set_speed(PORT_MOTOR_L, 0, 1);
nxt_motor_set_speed(PORT_MOTOR_R, 0, 1);
break;
}
ext_tsk(); /* terminates this task */
}
