int exynos_gpu_init_hw(struct kbase_device *kbdev)
{
unsigned int temp;
int i, count = 0;
static int first_call = 1;
struct exynos_context * platform = (struct exynos_context *)kbdev->platform_context;
/* G3D_CFGREG0 register bit16 set to 1.
* It is change the blkg3d clock path from aclk-g3d to aclk-g3d feed-back path */
temp = __raw_readl(EXYNOS7420_VA_SYSREG + 0x1234);
temp |= (0x1 << 16);
__raw_writel(temp, EXYNOS7420_VA_SYSREG + 0x1234);
/* MALI_SEC */
if(first_call == 1)
{
first_call = 0;
for(i = 0; i < 5; i++)
balance_init(kbdev);
}
else {
do {
if(balance_init(kbdev) == MALI_TRUE)
break;
if(count == 5)
break;
count++;
}while(1);
}
if (count > 0 && count < BMAX_RETRY_CNT)
platform->balance_retry_count[count -1] += 1;
return count;
}
void loadb_init( char* type )
{
sv.balance_name = type;
if(!strcmp(type, BALANCE_NONE)) sv.balance_type = BALANCE_NONE_T;
else if(!strcmp(type, BALANCE_STATIC1)) sv.balance_type = BALANCE_STATIC1_T;
else if(!strcmp(type, BALANCE_STATIC2)) sv.balance_type = BALANCE_STATIC2_T;
else if(!strcmp(type, BALANCE_STATIC3)) sv.balance_type = BALANCE_STATIC3_T;
else if(!strcmp(type, BALANCE_LIGHTEST)) sv.balance_type = BALANCE_LIGHTEST_T;
else if(!strcmp(type, BALANCE_SPREAD)) sv.balance_type = BALANCE_SPREAD_T;
else if(!strcmp(type, BALANCE_QUADTREE)) sv.balance_type = BALANCE_QUADTREE_T;
else if(!strcmp(type, BALANCE_AREATREE)) sv.balance_type = BALANCE_AREATREE_T;
else if(!strcmp(type, BALANCE_GRAPH)) sv.balance_type = BALANCE_GRAPH_T;
else if(!strcmp(type, BALANCE_AOIGRAPH)) sv.balance_type = BALANCE_AOIGRAPH_T;
else if(!strcmp(type, BALANCE_LOSGRAPH)) sv.balance_type = BALANCE_LOSGRAPH_T;
else
{
printf("Error: Unrecognized balance policy: %s\n", type );fflush(stdout);
assert(0);
}
balance_init();
}
// 倒立振子制御を行う
void BalanceControl::calculation(char forward, char turn, float gyro, float gyro_offset, long motor_ang_l, long motor_ang_r){
if(! isInitialized)
{
// 倒立振子制御初期化
balance_init();
// 初期化済にする
isInitialized = TRUE;
}
balance_control(
(float)forward,
(float)turn,
(float)gyro,
(float)gyro_offset,
(float)motor_ang_l,
(float)motor_ang_r,
(float)ecrobot_get_battery_voltage(),
(signed char*)&pwm_l,
(signed char*)&pwm_r
);
}
/* 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();
}
void seesaw_ride_pair( Command* cmd , ROBOT_STATUS* status )
{
signed char left;
signed char right;
f4 f4t_yp;
f4 f4t_yd;
static u2 u2_count = 0;
static u2 u2_gyro_temp=0;
static s4 s4t_distance = 0;
static s4 s4t_motor_L1 = 0;
static s4 s4t_motor_R1 = 0;
static s4 s4t_motor_L2 = 0;
static s4 s4t_motor_R2 = 0;
static s4 s4t_motor_L3 = 0;
static s4 s4t_motor_R3 = 0;
static s4 s4t_motor_L4 = 0;
static s4 s4t_motor_R4 = 0;
static s1 tail_seesaw = 0;
static u1 u1s_trace = 0;
static u1 u1s_touritu = 0;
s4t_motor_L1 = nxt_motor_get_count(PORT_MOTOR_L);
s4t_motor_R1 = nxt_motor_get_count(PORT_MOTOR_R);
static u4 u4_recieve = 0;
u1 receive_data[90] = {0};
/* user logic start */
u1s_2msflipcount_see = 1 - u1s_2msflipcount_see;
display_clear(0);
display_goto_xy(0, 0); display_string("stage"); display_unsigned(u1g_seesaw_ride_pair_mode,3);
display_goto_xy(0, 1); display_string("LEFT"); display_unsigned(left,3);
display_goto_xy(0, 2); display_string("RIGHT"); display_unsigned(right,3);
display_goto_xy(0, 3); display_string("count"); display_unsigned(u1s_touritu,3);
display_update();
if ( u1s_2msflipcount_see == 1 )
{
switch(u1g_seesaw_ride_pair_mode) /* シーソーシーケンス開始 */
{
case 0 : /* 倒立開始 */
// u1s_trace = 1; /* トレースOFF */
u1s_touritu = 1; /* トレースON */
f4g_forward = 0;
balance_init(); /* balance APIの初期化 */
nxt_motor_set_count(PORT_MOTOR_L, 0); /* 左モータ・カウント値[°]を0リセット */
nxt_motor_set_count(PORT_MOTOR_R, 0); /* 右モータ・カウント値[°]を0リセット */
s4t_motor_L2 = nxt_motor_get_count(PORT_MOTOR_L);
s4t_motor_R2 = nxt_motor_get_count(PORT_MOTOR_R);
//u1g_seesaw_ride_pair_mode = 1;
u2_count = 0;
tail_seesaw = tailControl( 30, status->motorangle_T);
if(status->motorangle_T > 10){
u1g_seesaw_ride_pair_mode = 1;
tail_seesaw = 0;
}
break;
// case 51:
// u1s_touritu = 0;
// break;
case 1:
if(status->lightval > GRAY_WHITE){
u1s_trace = 1;
f4g_forward = 20;
}else{
f4g_forward = 20;
chokushin2();
u1g_seesaw_ride_pair_mode = 2;
}
break;
case 2 : /* 段差検知まで */
s4t_distance = s4t_motor_L1 - s4t_motor_L2 + s4t_motor_R1 - s4t_motor_R2;
if(s4t_distance < 300){
u1s_trace = 1; /*トレースの設定*/
f4g_forward = 20;
}else{
if(f4g_turn < 3 && f4g_turn > -3){
u2_count++;
}else{
u2_count = 0;
}
}
if(u2_count > 10 && f4g_turn < 1 && f4g_turn > -1){
u1g_seesaw_ride_pair_mode++;
u2_count = 0;
f4g_turn = 0;
}
break;
case 3:
u1s_trace = 0;
f4g_forward = 30;
chokushin2();
if(status->gyroval > status->gyro_offset + 40 || status->gyroval < status->gyro_offset - 40){
u1g_seesaw_ride_pair_mode++;
f4g_forward = 100;
s4t_motor_L2 = nxt_motor_get_count(PORT_MOTOR_L);
s4t_motor_R2 = nxt_motor_get_count(PORT_MOTOR_R);
u2_count = 0;
u1s_trace = 0;
u2_gyro_temp = robot.robotSensor.GYRO.gyroOffsetValue - 5;
}
break;
case 4 : /* 検知後ある一定以上上る */
s4t_distance = s4t_motor_L1 - s4t_motor_L2 + s4t_motor_R1 - s4t_motor_R2;
if(s4t_distance < 20){ //ﾍﾞｰｽ100
f4g_forward = 100;
chokushin2();
}else if(s4t_distance < 60){ //べーす３００
robot.robotSensor.GYRO.gyroOffsetValue = u2_gyro_temp;
f4g_forward = 40;
chokushin2();
}else{
u2_count = 0;
f4g_forward = 0;
chokushin2();
u1g_seesaw_ride_pair_mode = 5;
}
break;
case 5: /* Bluetoothが来るまで待機 */
f4g_forward = 0;
chokushin2();
u4_recieve = ecrobot_read_bt_packet( receive_data, 90 );
if ( u4_recieve > 0 )
{
ecrobot_sound_tone(100, 100, 50);
u2_count = 0;
f4g_forward = 0;
chokushin2();
u1g_seesaw_ride_pair_mode++;
}
break;
case 6: /* Bluetoothが来たから上る */
s4t_distance = s4t_motor_L1 - s4t_motor_L2 + s4t_motor_R1 - s4t_motor_R2;
if(s4t_distance < 1060){
f4g_forward = 20;
chokushin2();
}else{
robot.robotSensor.GYRO.gyroOffsetValue = robot.robotSensor.GYRO.gyroOffsetValue -5;
u1g_seesaw_ride_pair_mode++;
// ecrobot_sound_tone(100, 100, 50);
}
break;
case 7 :
if(status->gyroval < status->gyro_offset - 80){
robot.robotSensor.GYRO.gyroOffsetValue = u2_gyro_temp;
u1g_seesaw_ride_pair_mode++;
nxt_motor_set_count(PORT_MO_TAIL, 0);
s4t_motor_L3 =nxt_motor_get_count(PORT_MOTOR_L);
s4t_motor_R3 =nxt_motor_get_count(PORT_MOTOR_R);
}else{
f4g_forward = 20;
chokushin2();
}
break;
case 8 : /* Bluetoothが来たから上る */
s4t_motor_L4 =s4t_motor_L3;
s4t_motor_R4 =s4t_motor_R3;
s4t_motor_L3 =nxt_motor_get_count(PORT_MOTOR_L);
s4t_motor_R3 =nxt_motor_get_count(PORT_MOTOR_R);
if((s4t_motor_R3 - s4t_motor_R4) < 0 && (s4t_motor_L3 - s4t_motor_L4)){
// u1s_touritu = 0;
// left = 0;
// right = 0;
// f4g_forward = 0;
// f4g_turn = 0;
u1g_seesaw_ride_pair_mode++;
}else{
f4g_forward = -60;
chokushin2();
}
break;
case 9 :
s4t_distance = s4t_motor_L1 - s4t_motor_L2 + s4t_motor_R1 - s4t_motor_R2;
if(s4t_distance > 1060){
f4g_forward = -20;
chokushin2();
}else{
u1g_seesaw_ride_pair_mode++;
// ecrobot_sound_tone(100, 100, 50);
}
case 10 :
u1s_touritu = 0;
left = 0;
right = 0;
f4g_forward = 0;
f4g_turn = 0;
break;
}
if (u1s_trace == 1)
{
f4t_yp = (f4)(GRAY_WHITE - status->lightval );
f4t_yd = (f4t_yp - f4s_last_yp) / 0.004;
f4s_last_yp = f4t_yp;
f4g_turn = (f4)((f4)TRACE_P * f4t_yp) + (f4)((f4)TRACE_D * f4t_yd);
// f4g_turn = -f4g_turn;
/* 旋回方向決定 */
if(f4g_turn > 50){
f4g_turn = 50;
}else if(f4g_turn < -50){
f4g_turn = -50;
}else{
f4g_turn = f4g_turn;
}
f4g_turn = -f4g_turn;
}
if (u1s_touritu == 1)
{
balance_control(
(f4)f4g_forward, /* 前後進命令(+:前進, -:後進) */
(f4)-f4g_turn, /* 旋回命令(+:右旋回, -:左旋回) */
// (f4)-6, /* 旋回命令(+:右旋回, -:左旋回) */
(f4)status->gyroval, /* ジャイロセンサ値 */
(f4)status->gyro_offset, /* ジャイロセンサオフセット値 */
(f4)status->motorangle_L, /* 左モータ回転角度[deg] */
(f4)status->motorangle_R, /* 右モータ回転角度[deg] */
(f4)status->batteryval, /* バッテリ電圧[mV] */
&left, /* 左モータPWM出力値 */
&right /* 右モータPWM出力値 */
);
setMotorVal( left , right , 0);
}else{
tail_seesaw = tailControl( 40, nxt_motor_get_count(PORT_MO_TAIL));
setMotorVal( left , right , tail_seesaw );
}
}
}
//走行状態設定
void RN_setting()
{
static float beforestop;
switch (setting_mode){
case (RN_SETTINGMODE_START):
RN_calibrate();
break;
case (RN_RUN):
RA_linetrace_PID(30);
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));
break;
case(RN_SLOW_RUN):
revL = nxt_motor_get_count(NXT_PORT_C);
revR = nxt_motor_get_count(NXT_PORT_B);
distance_slow = fabs(CIRCUMFERENCE/360.0 * ((revL+revR)/2.0));
if((distance_before - distance_slow <= -5))
{
gyro_offset -= 15;
balance_init();
nxt_motor_set_count(NXT_PORT_B,0);
nxt_motor_set_count(NXT_PORT_C,0);
setting_mode = RN_STOP_WAIT;
}
else
{
RA_speed(-50,8);
cmd_turn = RA_wheels(cmd_turn);
}
break;
case(RN_BACK_RUN):
RA_linetrace_PID(-15);
break;
case(RN_HIGH_RUN):
break;
case(RN_STOP_WAIT):
revL = nxt_motor_get_count(NXT_PORT_C);
revR = nxt_motor_get_count(NXT_PORT_B);
distance_stop = fabs(CIRCUMFERENCE/360.0 * ((revL+revR)/2.0));
beforestop = distance_stop;
wait_count += 1;
RA_linetrace(0,2);
if(wait_count == 550)
{
balance_init();
nxt_motor_set_count(NXT_PORT_B,0);
nxt_motor_set_count(NXT_PORT_C,0);
RA_hensareset();
wait_count = 0;
gyro_offset -= 15;
setting_mode = RN_RUN;
runner_mode = RN_MODE_CONTROL;
tail_mode = RN_TAILUP;
}
break;
case(RN_STOP):
nxt_motor_set_speed(NXT_PORT_C, 0, 1);
nxt_motor_set_speed(NXT_PORT_B, 0, 1);
break;
case(RN_LITTLE_RUN):
wait_count += 1;
mode_count += 1;
RA_linetrace(5,10);
if(wait_count >= 500)
{
wait_count = 0;
setting_mode = RN_LITTLE_LOW;
}
if(mode_count >= 5000)
{
mode_count = 0;
balance_init();
nxt_motor_set_count(NXT_PORT_B,0);
nxt_motor_set_count(NXT_PORT_C,0);
RA_hensareset();
wait_count = 0;
setting_mode = RN_RUN;
runner_mode = RN_MODE_CONTROL;
tail_mode = RN_TAILUP;
}
break;
case(RN_LITTLE_LOW):
wait_count += 1;
mode_count += 1;
RA_linetrace(-3,2);
if(wait_count >= 500)
{
wait_count = 0;
setting_mode = RN_LITTLE_RUN;
}
if(mode_count >= 5000)
{
mode_count = 0;
balance_init();
nxt_motor_set_count(NXT_PORT_B,0);
nxt_motor_set_count(NXT_PORT_C,0);
RA_hensareset();
wait_count = 0;
setting_mode = RN_RUN;
runner_mode = RN_MODE_CONTROL;
tail_mode = RN_TAILUP;
}
break;
default:
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 */
}
/**
* バランサを初期化する
* @param offset ジャイロセンサオフセット値
*/
void Balancer::init(int offset) {
mOffset = offset;
balance_init(); // 倒立振子制御初期化
}
/**
* BalancerUnitの初期化処理
*/
void BLNU_pre(void) {
ecrobot_set_motor_rev(L_MOTOR_PORT, 0);
ecrobot_set_motor_rev(R_MOTOR_PORT, 0);
balance_init();
BLNU_motor_angle_control = false;
}
