自分なりの解説
レーンチェンジ
通常トレース
case 11:
/* 通常トレース */
gain = 10;
i = getServoAngle(); // SERVO_STEP;
led_out(0xff);
servoPwmOut( iServoPwm );
if( i >= 130 ) {//170
motor_f( 0, 0 );
motor_r( 0, 0 );
}
else if( i >= 60 ) {
if (iEncoder <= TESTSPEED){ // 35 50
motor_f( diff(80), 80 ); //50
motor_r( diff(80), 80 ); //50
}
else{
motor_f( 0, 0 );
motor_r( 0, 0 );
}
}
else if( i >= 40 ) {
if (iEncoder <= TESTSPEED){ //40 55
motor_f( diff(90), 90 ); //50
motor_r( diff(90), 90 ); //50
}
else{
motor_f( 0, 0 );
motor_r( 0, 0 );
}
}
else if( i >= 20 ) {
if (iEncoder <= TESTSPEED){ //45 60
motor_f( diff(100), 100 );
motor_r( diff(100), 100 );
}
else{
motor_f( 0, 0 );
motor_r( 0, 0 );
}
}
else if( i <= -130 ) {//-170
motor_f( 0, 0 );
motor_r( 0, 0 );
}
else if( i <= -60 ) {
if (iEncoder <= 50){//35
motor_f( 80, diff(80) );
motor_r( 80, diff(80) );
}
else{
motor_f( 0, 0 );
motor_r( 0, 0 );
}
}
else if( i <= -40 ) {
if (iEncoder <= TESTSPEED){ //40 55
motor_f( 90, diff(90) );
motor_r( 90, diff(90) );
}
else{
motor_f( 0, 0 );
motor_r( 0, 0 );
}
}
else if( i <= -20 ) {
if(iEncoder <= TESTSPEED){ //45 60
motor_f( 100, diff(100) );
motor_r( 100, diff(100) );
}
else{
motor_f( 0, 0 );
motor_r( 0, 0 );
}
}
else {
if (iEncoder <= TESTSPEED){ //50 70
motor_f( 100, 100 );
motor_r( 100, 100 );
}
else{
motor_f( 0, 0 );
motor_r( 0, 0 );
}
}
if(center_inp() == 0x00){
led_out(0x00);
}
else {
led_out(0xff);
}
if( check_crossline()) { /* クロスラインチェック */
servoPwmOut( 0 );
lEncoderCrank = lEncoderTotal;
causeout = lEncoderTotal;
led_out(0x0f);
pattern = 21;
break;
}
if( check_zlineR()) { // 右車線変更ラインチェック
servoPwmOut( 0 );
lEncoderCrank = lEncoderTotal;
causeout = lEncoderTotal;
Lane_Change = RIGHT; // 1:左 0:右
led_out( 0x33 );
pattern = 51;
break;
}
if( check_zlineL()) { // 左車線変更ラインチェック
servoPwmOut( 0 );
lEncoderCrank = lEncoderTotal;
causeout = lEncoderTotal;
Lane_Change = LEFT; // 1:左 0:右
led_out( 0x44 );
pattern = 51;
break;
}
if( !check_Noline() ){
causeout = lEncoderTotal;
}
if( pushsw_get() && cnt1 >= 1000) {
M_FreeMoter = 0;
setBeepPatternS( 0xcc00 );
cnt1 = 0;
pattern = 6;
break;
}
クロスラインのチェックと同様にcheck_zlineR()の関数でハーフラインを読みます
check_zlineL()なら左、check_zlineR()なら右です。
クランクチェック
case 51: // クランクチェック
gain = 10;
servoPwmOut( 0 );
if (iEncoder <= TESTSPEED){//43 50
motor_f( 60, 60 );
motor_r( 60, 60 );
}
else if (iEncoder > TESTSPEED){
motor_f( 0, 0);
motor_r( 0, 0);
}
if(((lEncoderTotal - lEncoderCrank) <= 300) && check_crossline()) {
lEncoderCrank = lEncoderTotal;
causeout = lEncoderTotal;
pattern = 21;
break;
}
else if((lEncoderTotal - lEncoderCrank) > 300) { // 10mmで3
lEncoderCrank = lEncoderTotal;
causeout = lEncoderTotal;
pattern = 52;
break;
}
if( !check_Noline() ){
causeout = lEncoderTotal;
}
break;
case 52: // ライン終了サーチ
gain = 10;
servoPwmOut( iServoPwm );
if (iEncoder <= TESTSPEED){//45 50
motor_f( 80, 80 );
motor_r( 80, 80 );
}
else if (iEncoder > TESTSPEED){
motor_f( 0, 0); //-100
motor_r( 0, 0);
}
if( check_Noline() && Lane_Change == RIGHT ) { // 右車線変更?
crank_mode = 1;
lEncoderCrank = lEncoderTotal;
causeout = lEncoderTotal;
led_out( 1 );
pattern = 61;
break;
} else if( check_Noline() && Lane_Change == LEFT ) { // 左車線変更？
crank_mode = 1;
lEncoderCrank = lEncoderTotal;
causeout = lEncoderTotal;
led_out( 2 );
pattern = 71;
break;
}
causeout = lEncoderTotal;
break;
case 51では
ハーフラインを踏んでから300以内にクロスライン検地をしたらクランクに切り替わるようにしています。
300を超えたらレーンチェンジとして判定します。
case 52では
白線がなくなった時に角度を切るようにしています
RIGHTなら右、LEFTなら左です。
右レーンチェンジ
case 61: // (右)車線変更処理1
handle( 50 );
if (iEncoder <= TESTSPEED){//43 50
motor_f( 80, 70 );
motor_r( 80, 70 );
}
else if (iEncoder > TESTSPEED){
motor_f( 0, 0);
motor_r( 0, 0);
}
if( (sensor_inp() == 0x01) && (sensor_inp() != 0x08) ){ // デジタル1,2,4 アナログL,R タイヤ側から見て
iSensorPattern = 0;
crank_mode = 0;
lEncoderCrank = lEncoderTotal;
causeout = lEncoderTotal;
pattern = 62;
break;
}
break;
case 62: // (右)車線変更処理2
handle( 1 );
if (iEncoder <= TESTSPEED){//43 50
motor_f( 80, 80 );
motor_r( 80, 80 );
}
else if (iEncoder > TESTSPEED){
motor_f( 0, 0);
motor_r( 0, 0);
}
if( (sensor_inp() == 0x08 || sensor_inp() == 0x04) ){//&& (sensor_inp() != 0x01)) {
lEncoderCrank = lEncoderTotal;
causeout = lEncoderTotal;
pattern = 63;
break;
}
break;
case 63: // (右)車線変更処理3
handle( 25 );
if (iEncoder <= TESTSPEED){//43 50
motor_f( 70, 80 );
motor_r( 70, 80 );
}
else if (iEncoder > TESTSPEED){
motor_f( 0, 0);
motor_r( 0, 0);
}
if( sensor_inp() == 0x02 || sensor_inp() == 0x01 || center_inp() == 0x01 ) {
lEncoderCrank = lEncoderTotal;
causeout = lEncoderTotal;
pattern = 64;
break;
}
break;
case 64: // (右)車線変更処理4
gain = 10;
servoPwmOut( iServoPwm );
if (iEncoder <= TESTSPEED){//43 50
motor_f( 60, 60 );
motor_r( 60, 60 );
}
else if (iEncoder > TESTSPEED){
motor_f( 0, 0);
motor_r( 0, 0);
}
if( (lEncoderTotal - lEncoderCrank) >= 25 ) { // 10mmで3 150mm後に通常トレースへ遷移
lEncoderCrank = lEncoderTotal;
causeout = lEncoderTotal;
pattern = 11;
break;
}
if( !check_Noline() ){
causeout = lEncoderTotal;
}
break;
case 61から64までは反応しているセンサーに合わせてハンドルを切っています。
考え方はクランクとほとんど同じです。
左レーンチェンジ
case 71: // (左)車線変更処理1
handle( -50 );
if (iEncoder <= TESTSPEED){//43 50
motor_f( 70, 80 );
motor_r( 70, 80 );
}
else if (iEncoder > TESTSPEED){
motor_f( 0, 0);
motor_r( 0, 0);
}
if( (sensor_inp() == 0x08) && (sensor_inp() != 0x01)){
iSensorPattern = 0;
crank_mode = 0;
lEncoderCrank = lEncoderTotal;
causeout = lEncoderTotal;
pattern = 72;
break;
}
break;
case 72: // (左)車線変更処理2
handle( -1 );
if (iEncoder <= TESTSPEED){//43 50
motor_f( 80, 80 );
motor_r( 80, 80 );
}
else if (iEncoder > TESTSPEED){
motor_f( 0, 0);
motor_r( 0, 0);
}
if( (sensor_inp() == 0x01 || sensor_inp() == 0x02)){ // && (sensor_inp() != 0x08)) {
lEncoderCrank = lEncoderTotal;
causeout = lEncoderTotal;
pattern = 73;
break;
}
break;
case 73: // (左)車線変更処理3
handle( 25 );
if (iEncoder <= TESTSPEED){//43 50
motor_f( 80, 70 );
motor_r( 80, 70 );
}
else if (iEncoder > TESTSPEED){
motor_f( 0, 0);
motor_r( 0, 0);
}
if( sensor_inp() == 0x04 || sensor_inp() == 0x08 || center_inp() == 0x01) {
lEncoderCrank = lEncoderTotal;
causeout = lEncoderTotal;
pattern = 74;
break;
}
break;
case 74: // (左)車線変更処理4
gain = 10;
servoPwmOut( iServoPwm );
if (iEncoder <= TESTSPEED){//43 50
motor_f( 60, 60 );
motor_r( 60, 60 );
}
else if (iEncoder > TESTSPEED){
motor_f( 0, 0);
motor_r( 0, 0);
}
if((lEncoderTotal - lEncoderCrank) >= 25 ) { // 10mmで3 150mm後に通常トレースへ遷移
lEncoderCrank = lEncoderTotal;
causeout = lEncoderTotal;
pattern = 11;
break;
}
if( !check_Noline() ){
causeout = lEncoderTotal;
}
break;
case 71から74までは反応しているセンサーに合わせてハンドルを切っています。
考え方はクランクとほとんど同じです。
角度
/************************************************************************/
/* モジュール名 handle */
/* 処理概要 サーボモータ制御 角度指定用 */
/* 引数 なし */
/* 戻り値 グローバル変数 servoPwmOut に代入 */
/************************************************************************/
void handle( int iSetAngle ){
int i, j, iRet, iP, iD ;
i = - iSetAngle; /* 設定したい角度 */
//j = ( p7_2 >>2 );
j = getServoAngle(); /* 現在の角度 */
/* サーボモータ用PWM値計算 */
iP = 10 * (j - i); /* 比例 */
iD = 100 * (iAngleBefore2 - j); /* 微分 */
iRet = iP - iD;
iRet = iRet / (7/2); //iRet /= 4; //2
if( iRet > 120 ) iRet = 120; /* マイコンカーが安定したら */
if( iRet < -120 ) iRet = -120; /* 上限を90くらいにしてください */
servoPwmOut(iRet);
iAngleBefore2 = j;
}
サンプルには入っていませんが、解説には入っています。
使う変数も一式書いてあるため、そちらをご覧ください。
ハーフラインの検知
/************************************************************************/
/* 右車線変更ライン検出処理 */
/* 引数 なし */
/* 戻り値 0:車線変更ラインなし 1:あり */
/************************************************************************/
int check_zlineR( void )
{
int ret;
ret = 0;
// if((sensor_inp()==0x03) || (sensor_inp()==0x01)) {
//sensor_inp()!=0x09 || sensor_inp()!=0x05 || sensor_inp()!=0x0a || sensor_inp()!=0x0f)
if((sensor_inp()==0x01 || sensor_inp()==0x03)
&& center_inp() == 0x01){
ret = 1;
}
return ret;
}
/************************************************************************/
/* 左車線変更ライン検出処理 */
/* 引数 なし */
/* 戻り値 0:車線変更ラインなし 1:あり */
/************************************************************************/
int check_zlineL( void )
{
int ret;
ret = 0;
// if((sensor_inp()==0x0c) || (sensor_inp()==0x08)) {
///sensor_inp()!=0x09 || sensor_inp()!=0x05 || sensor_inp()!=0x0a || sensor_inp()!=0x0f)
if((sensor_inp()==0x08 || sensor_inp()==0x0c)
&& center_inp() == 0x01){
ret = 1;
}
return ret;
}
真ん中を除く4つのデジタルセンサを使っているため16進数で表すことができます。
左を検知したいのならば0x08や0x0c、右を検知したいのならば0x01や0x03が良いと思います。
center_inp()は真ん中のセンサーです。
1なら反応しています。