Exemple #1
0
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;
}
Exemple #2
0
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
	);
}
Exemple #4
0
/* 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();
}
Exemple #5
0
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){    //べーす300

                    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;
	}
}
Exemple #7
0
//*****************************************************************************
// 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 */
}
Exemple #8
0
/**
 * バランサを初期化する
 * @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;
}