Esempio n. 1
0
/**
 * 目標距離と現在距離から、減速動作を行うのに適切な走行ベクトルを計算する
 *
 * @return 走行ベクトル
 */
VectorT<float> SlowdownSkill::calcCommand()
{
    // スキルの利用。フォワード値を(必要があれば)上書きする。
    VectorT<float> command = mSkill->calcCommand();

    // フォワード値のPID制御
    float P = mTargetDistance -  mGps.getDistance();
    float X = mSlowdownPid.control(P);
    if (X > command.mX) X = command.mX;
    if (X < mMinimumForward) X = mMinimumForward;
    command.mX = X;

#if 0
    //DESK_DEBUG = true; // モータを回さないデバグ
    static int count = 0; // 今だけ static
    if (count++ % 25 == 0) {
        Lcd lcd;
        lcd.clear();
        lcd.putf("sn", "SlowdownSkill");
        lcd.putf("dn", (int)mTargetDistance);
        lcd.putf("dn", (int)mAllowableError);
        lcd.putf("dn", (int)mGps.getDistance());
        lcd.putf("dn", (int)P);
        lcd.putf("dn", (int)command.mX);
        lcd.disp();
    }
#endif

	return command;
}
Esempio n. 2
0
//=============================================================================
// Sub functions
static void connect_bt(Lcd &lcd, char bt_name[16])
{
    //CHAR  name[16] = "Kachi";
    U8 address[7];

    lcd.clear();

    if (mBluetooth.getDeviceAddress(address)) // get the device address
    {
        lcd.putf("sn", "BD_ADDR:");
        for (SINT i=0; i<7; i++) lcd.putf("x", address[i],2);

        mBluetooth.setFriendlyName(bt_name); // set the friendly device name
        if (mBluetooth.getFriendlyName(bt_name)) // display the friendly device name
        {
            lcd.putf("nssn", "BD_NAME: ", bt_name);
        }

        lcd.putf("nsn", "Connecting BT...");
        lcd.putf("sn",  "ENTR to cancel.");
        lcd.disp();

        if (mBluetooth.waitForConnection("1234", 0)) // wait forever
        {
            lcd.putf("s", "Connected.");
        }
    }
    else
    {
        lcd.putf("s", "BT Failed.");
    }

    lcd.putf("ns", "Press Touch.");
    lcd.disp();
}
Esempio n. 3
0
/**
 * 走行。ハンドル、アクセルの操作。
 *
 * @param[in] command 走行ベクトル(forward, turn)
 */
void TripodActivator::run(VectorT<F32> command)
{
    float pwm_L, pwm_R;

    // フォワードPID
    if (gDoForwardPid) {
        command.mX = forwardPid(command.mX);
    }

    // 過去の蓄積ベースのturn値
    if (gDoProgressiveTurn) {
        command.mY += mTurnHistory.calcAverage();
        mTurnHistory.update(command.mY);
    }

    // @todo: balance_control と同じ入力値なら同じぐらいの出力値になるようにしたい
    //pwm_L = command.mX + (command.mY > 0 ? command.mY : 0) * 0.5;
    //pwm_R = command.mX + (-command.mY > 0 ? -command.mY : 0) * 0.5;
    pwm_L = command.mX + command.mY * 0.5;
    pwm_R = command.mX - command.mY * 0.5;

    if (! DESK_DEBUG) {
        mLeftMotor.setPWM((S8)(MIN(MAX(pwm_L, -128), 127)));
        mRightMotor.setPWM((S8)(MIN(MAX(pwm_R, -128), 127)));
    }

    // tail_control
    tail_control(TAIL_ANGLE_TRIPOD_DRIVE);

#if 0 // DEBUG
    static int count = 0;
    if (count++ > 5) {
        Lcd lcd;
        lcd.clear();
        lcd.putf("sn", "TripodActivator");
        lcd.putf("dn", (int)command.mX);
        lcd.putf("dn", (int)command.mY);
        lcd.putf("dn", (int)pwm_L);
        lcd.putf("dn", (int)pwm_R);
        lcd.disp();
    }
#endif
}
Esempio n. 4
0
void tsk3(VP_INT exinf)
{
	while(1)
	{
		lcd.cursor(0, 4);
		lcd.putf("sd", "Task3: ", clock.now(),8);

		clock.sleep(2999); // sleep over 2999 msec
	}
}
Esempio n. 5
0
void tsk2(VP_INT exinf)
{
	while(1)
	{
		lcd.cursor(0, 3);
		lcd.putf("sd", "Task2: ", clock.now(),8);

		clock.sleep(1999); // sleep over 1999 msec
	}
}
Esempio n. 6
0
void tsk1(VP_INT exinf)
{
	while(1)
	{
		lcd.cursor(0, 2);
		lcd.putf("sd", "Task1: ", clock.now(),8);
		lcd.disp();

		clock.sleep(999); // sleep over 999 msec
	}
}
Esempio n. 7
0
	/* nxtOSEK hook to be invoked from an ISR in category 2 */
	void user_1ms_isr_type2(void)
	{
		StatusType ercd = SignalCounter(SysTimerCnt); /* Increment OSEK Alarm Counter */
	    if(ercd != E_OK)
	    {
		    lcd.clear();
			lcd.putf("sn",   "Shutdown OS!");
			lcd.disp();
			clock.wait(2000);
		    ShutdownOS(ercd);
	    }
		SleeperMonitor(); // needed for I2C device and Clock classes
	}
Esempio n. 8
0
void tsk_ini(VP_INT exinf)
{
	lcd.clear();
	lcd.putf("s", "Clock");
	
	clock.sleep(1); // wait for next tick
	
	clock.reset();
	
	act_tsk(TSK1);
	act_tsk(TSK2);
	act_tsk(TSK3);
}
bool TestDriver::drive()
{
#if 0 // ログ送信
    LOGGER_SEND = 2;
	LOGGER_DATAS08[0] = (S8)(mLineDetector.detect());
	LOGGER_DATAS16[0] = (S16)(mGps.getXCoordinate());
	LOGGER_DATAS16[1] = (S16)(mGps.getYCoordinate());
	LOGGER_DATAS16[2] = (S16)(mGps.getDirection());
	LOGGER_DATAS16[3] = (S16)(mGps.getDistance());
	LOGGER_DATAS32[0] = (S32)(mLightHistory.calcDifference());
#endif
#if 1 // DEBUG
    //DESK_DEBUG = true; // モータを回さないデバグ
    static int count = 0; // staticは原則禁止だが今だけ
    if (count++ % 25 == 0) {
        Lcd lcd;
        lcd.clear();
        lcd.putf("sn", "TestDriver");
        lcd.putf("dn", (S32)(mGps.getXCoordinate()));
        lcd.putf("dn", (S32)(mGps.getYCoordinate()));
        lcd.putf("dn", (S32)(mGps.getDirection()));
        lcd.putf("dn", (S32)(mGps.getDistance()));
        lcd.putf("dn", (S32)(mLeftMotor.getCount()));
        lcd.putf("dn", (S32)(mRightMotor.getCount()));
        //lcd.putf("dn", (S32)(mLineDetector.detect()));
        //lcd.putf("dn", (S32)(mLightHistory.calcDifference()));
        lcd.disp();
    }
#endif
    // デフォルト
    tail_control(TAIL_ANGLE_DRIVE); /* バランス走行用角度に制御 */
    VectorT<float> command(50, 0);

    // テスト 通常走行
    if (0) {
        mActivator.run(command);
    }
    // テスト 3点走行
    if (0) {
        tail_control(TAIL_ANGLE_TRIPOD_DRIVE); /* 3点走行用角度に制御 */
        mTripodActivator.run(command);
    }
    // テスト 3点走行 with フォワードPID
    if (0) {
        gDoForwardPid = true;
        tail_control(TAIL_ANGLE_TRIPOD_DRIVE); /* 3点走行用角度に制御 */
        mTripodActivator.run(command);
    }
    // テスト 3点走行ライントレース
    if (1) {
        tail_control(TAIL_ANGLE_TRIPOD_DRIVE); /* 3点走行用角度に制御 */
        mTripodLineTrace.setForward(50);
        mTripodLineTrace.execute();
    }

    return true;
}
Esempio n. 10
0
bool DashedLineSection::execute() {
    static int state = -1;
    static int state1Flagcount = 0;
    //static int secondToThirdFlagcount = 0;
    VectorT<S8> velocity;

#if PRINT_STATE
    Lcd lcd;
    lcd.clear();
    lcd.putf("sn",  "DashedState");
    lcd.putf("sdn", "SSID", state);
    lcd.disp();
#endif
    
    if (state == -1) { // initialization state
        distance.reset();
        state = 0;
    }
    if (state == 0) {
        velocity = directionPidDriver.computeVelocity(DASHED_LINE_STRATEGY_STRAIGHT_DIRECTION_TO_GO);
    	velocity.mX = DASHED_LINE_STRATEGY_STRAIGHT_DIRECTION_TO_GO_SPEED;
        
        if(distance.computeDistance() > DASHED_LINE_STRATEGY_DISTANCE_TO_LAND){
            state = 1;
        }
    } else if (state == 1) {
        velocity = directionPidDriver.computeVelocity(DASHED_LINE_STRATEGY_STRAIGHT_DIRECTION_TO_LAND);
    	velocity.mX = DASHED_LINE_STRATEGY_STRAIGHT_DIRECTION_TO_LAND_SPEED;
        countFlag(state1Flagcount, history.getLightSensorHistory() > WHITE_GRAY_THRESH);
        if(state1Flagcount > 1) {
            this->endFlag = true;
        }
    }

    if (this->isEnd()) {
        return true;
    } else {
#if TORQUE_MOTOR
        bRun.run(velocity);
#endif
        return false;
    }
};
Esempio n. 11
0
    VectorT<S8> computeVelocity(float refDirection)
    {
        VectorT <S8> velocity;
        F32 curDirection = direction.getDirectionOff();

        // Y そのものバージョン
        float Y = anglePid.control(curDirection, refDirection);
        velocity = this->controlVelocity(Y);

        // dY バージョン
        // (全然だめ。むしろ目標値付近でYの値が大きくなる)
        //anglePid.setKp(0.001);
        //anglePid.setKi(0.0);
        //anglePid.setKd(0.0);
        //static float Y = 0.0;
        //float dY = anglePid.control(curDirection, refDirection);
        //Y += dY;
        //velocity = this->controlVelocity(Y);
        
#if PRINT_DIRECTION_PID_DRIVER
        DEBUG_DATA_LONG = (long)curDirection;
        Lcd lcd;
        lcd.clear();
        lcd.putf("sn",  "DirectionPidDriver");
        lcd.putf("sdn", "RDIR", (int)refDirection);
        lcd.putf("sdn", "CDIR", (int)curDirection);
        lcd.putf("sdn", "Raw ", (int)direction.getDirection());
        lcd.putf("sdn", "RawO", (int)direction.getDirectionOff());
        lcd.putf("sdn", "Avg ", (int)direction.getAvgDirection());
        lcd.putf("sdn", "AvgO", (int)direction.getAvgDirectionOff());
        lcd.putf("sdn", "OFF ", (int)direction.getOffset());
        lcd.putf("sdn", "   Y", (int)Y);
        lcd.disp();
#endif
        return velocity;
    }
Esempio n. 12
0
float PidController::control(float current, float reference)
{
    static float buf[MAX_SIZE] = {0};
    static unsigned int index = 0;
    static float P, I = 0, D, PP = 0;

    // Proportional
    P = current - reference;

    // Nothing but buffering (Ring bufffer)
    PP = buf[index]; // oldest value
    buf[index] = P;  // newest value
    index = (index + 1) % size;
    
    // Integral
    //I += P; // バッファオーバフローの危険性
    I = 0;
    for (int i = 0; i < size; i++) {
        I += buf[i];
    }

    //Derivative
    D = P - PP;

    float Y = ((kp*P) + (ki*I) + (kd*D));
#if PRINT_PID_CONTROLLER
    Lcd lcd;
    lcd.clear();
    lcd.putf("sn", "pid");
    lcd.putf("dn", (S32)(kp * 1000));
    lcd.putf("dn", (S32)(ki * 1000));
    lcd.putf("dn", (S32)(kd * 1000));
    lcd.putf("dn", (S32)(kp*P * 1000));
    lcd.putf("dn", (S32)(ki*I * 1000));
    lcd.putf("dn", (S32)(kd*D * 1000));
    lcd.putf("dn", (S32)(Y * 1000));
    lcd.disp();
#endif
    return Y;
};
Esempio n. 13
0
bool NewGoalStopSection::execute()
{
    Lcd lcd;
    static int state = -1;
    VectorT<S8> velocity;
    static F32 refDirection;

    lcd.putf("n");
    lcd.putf("sn", "NewGoalState ");
    lcd.putf("dn", state);
    lcd.disp();

    if (state == -1) { // initialization state
        this->distance.reset();
        state = 0;
    }
    if (state == 0) {
        velocity = this->computeRunningVelocity();
        
        if(this->distance.computeDistance() > distanceToGoal){
            state = 11;
            this->time.reset();
            this->distance.reset();
            refDirection = direction.getAvgDirectionOff();
        }
    } else if(state == 11) {
        velocity = directionPidDriver.computeVelocity(refDirection+90);
        velocity.mX = 80;
        beep();
        if(this->distance.computeDistance() > 30){
            state = 1;
            this->time.reset();
            this->distance.reset();
        }
    }
    else if (state == 1) { // 停止状態
        velocity = this->computeStopVelocity(refDirection);
        if (this->time.elapsedTime() > 7000/*GOAL_STOP_STRATEGY_FIRST_STOP_TIME*/) {
            state = 2;
            this->distance.reset();
        }
    } else if (state == 2) { // ライントレース
        velocity = this->computeRebootVelocity(refDirection);
        if(this->distance.computeDistance() > GOAL_STOP_STRATEGY_REPEATED_DISTANCE){
            state = 3;
            this->time.reset();
            this->distance.reset();
        }
    } else if (state == 3) { // 停止状態
        velocity = this->computeStopVelocity(refDirection);
        if (this->time.elapsedTime() > GOAL_STOP_STRATEGY_SECOND_STOP_TIME) {
            state = 4;
            this->distance.reset();
        }
    } else if (state == 4) { // ライントレース
        velocity = this->computeRebootVelocity(refDirection);
        if(this->distance.computeDistance() > GOAL_STOP_STRATEGY_REPEATED_DISTANCE){
            state = 5;
            this->time.reset();
            this->distance.reset();
        }
    } else if (state == 5) { // 停止状態
        velocity = this->computeStopVelocity(refDirection);
    }
#if TORQUE_MOTOR
    bRun.run(velocity);
#endif
    return false;
}
Esempio n. 14
0
/**
 * 適切なドライバを選択し、運転させる
 */
void OutCourse::drive()
{
#if 0 // ログ送信(0:解除、1:実施)
    LOGGER_SEND = 2;
    LOGGER_DATAS08[0] = (S8)(mState);
    LOGGER_DATAS08[1] = (S8)(mLineDetector.detect()); // 一瞬だけなのでログに残らない可能性あり
    LOGGER_DATAU16    = (U16)(mWallDetector.detect());
    LOGGER_DATAS16[0] = (S16)(mGps.getXCoordinate());
    LOGGER_DATAS16[1] = (S16)(mGps.getYCoordinate());
    LOGGER_DATAS16[2] = (S16)(mGps.getDirection());
    LOGGER_DATAS16[3] = (S16)(mGps.getDistance());
    LOGGER_DATAS32[0] = (S32)(mLeftMotor.getCount());
    LOGGER_DATAS32[1] = (S32)(mRightMotor.getCount());
    LOGGER_DATAS32[2] = (S32)(mLightSensor.get());
    LOGGER_DATAS32[3] = (S32)(mGyroSensor.get());
#endif
#if 0 // デバッグ(0:解除、1:実施)
    {
        //DESK_DEBUG = true;
        static int count = 0;
        if (count++ % 25 == 0) {
            Lcd lcd;
            lcd.clear();
            lcd.putf("sn", "OutCourse");
            lcd.putf("dn", mState);
            lcd.putf("dn", (S32)mGps.getXCoordinate());
            lcd.putf("dn", (S32)mGps.getYCoordinate());
            lcd.putf("dn", (S32)mGps.getDirection());
            lcd.putf("dn", (S32)mGps.getDistance());
            lcd.disp();
        }
    }
#endif
    if (mState == OutCourse::START) { // スタート後通常区間
        if (mNormalDriver.drive()) {
            float X = mGps.getXCoordinate();
            float Y = mGps.getYCoordinate();
            //if (inRegion(GPS_LOOKUP_START, MakePoint(X, Y)) || (13500.0 < mGps.getDistance())) { // 区間をルックアップ区間に更新
            //if (12500.0 < mGps.getDistance()) { // ルックアップ区間前減速
            //    mLineTrace.setForward(30);//ノーマルドライバ内でやる
            //}
            if (13500.0 < mGps.getDistance()) { // 区間をルックアップ区間に更新
                mState = OutCourse::LOOKUP;
            }
        }
    }
    else if (mState == OutCourse::LOOKUP) { // ルックアップ区間
        if (mLookUpGateDriver.drive()) {
            mState = OutCourse::ETSUMO;
            /*とりあえず終了したら遷移することにする
            float X = mGps.getXCoordinate();
            float Y = mGps.getYCoordinate();
        	if ((inRegion(GPS_ETSUMO_START, MakePoint(X, Y)))) { // 区間をET相撲区間に更新
                mState = OutCourse::ETSUMO;
            }
            */
        }
    }
    else if (mState == OutCourse::ETSUMO) { // ET相撲区間
        if (mETsumoDriver.drive()) {
            float X = mGps.getXCoordinate();
            float Y = mGps.getYCoordinate();
            if (inRegion(GPS_GARAGEIN_START, MakePoint(X, Y))) { // 区間をガレージ区間に更新
                mState = OutCourse::GARAGEIN;
            }
        }
    }
    else if (mState == OutCourse::GARAGEIN) { // ガレージ・イン区間
        mGarageDriver.drive();
    }
    // テストドライバ起動
    else {
        mTestDriver.drive();
    }
}