//=============================================================================
// 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();
}
/**
* 目標距離と現在距離から、減速動作を行うのに適切な走行ベクトルを計算する
*
* @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;
}
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);
}
// テスト ３点走行
if (0) {
tail_control(TAIL_ANGLE_TRIPOD_DRIVE); /* ３点走行用角度に制御 */
mTripodActivator.run(command);
}
// テスト ３点走行 with フォワードPID
if (0) {
gDoForwardPid = true;
tail_control(TAIL_ANGLE_TRIPOD_DRIVE); /* ３点走行用角度に制御 */
mTripodActivator.run(command);
}
// テスト ３点走行ライントレース
if (1) {
tail_control(TAIL_ANGLE_TRIPOD_DRIVE); /* ３点走行用角度に制御 */
mTripodLineTrace.setForward(50);
mTripodLineTrace.execute();
}
return true;
}
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
}
}
/* 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
}
/**
* 走行。ハンドル、アクセルの操作。
*
* @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
}
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;
}
};
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;
};
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;
}
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;
}
/**
* 適切なドライバを選択し、運転させる
*/
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();
}
}