みなさんこんにちは。
今回は、picマイコンと近接センサとラインセンサを用いた掃除ロボットのプログラムを紹介します。
課題
机の上にある障害物を避けながら、ゴミを回収する掃除ロボットを作成する。
机の上から落ちるのもダメ。
センサーは近接センサーとラインセンサーを使うことができる。
使用した電子部品
近接センサー:シャープ GP2Y0A21YK
ラインセンサー:浜松フォトニクス S6986
マイコン:PIC24FV32KA302
近接センサーはロボットの前方に2つ設置。
ラインセンサーはロボットの前方角に1つずつ設置。
プログラム
/*
* File: main4.c
* Author: ha211469
*
* Created on 2023/12/29, 14:05
*/
#include "xc.h"
_FOSCSEL(SOSCSRC_DIG)
_FOSC(OSCIOFNC_OFF)
_FWDT(FWDTEN_OFF)
void delay_ms(unsigned int ms)/*ミリ秒遅延のための関数*/
/*msの最大有効値は2097*/
{
TMR1 =0;
T1CONbits.TCKPS = 2;
PR1 =(1000l*ms)/32;
T1CONbits.TON = 1;
_T1IF = 0;
while(!_T1IF);
}
int main(void) {
/*I/Oポート*/
TRISA = 0;/*PORTAの全体を出力モードに設定*/
LATA = 0;/*データの書き込み0を出力*/
TRISB = 0;/*PORTBの全体を出力モードに設定*/
LATB = 0;/*データの書き込み0を出力*/
/*光変調ICの出力を読み出す*/
unsigned int left,right;
_TRISB0 = 1;/*RB0を入力モードに*/
_TRISB1 = 1;/*RB1を入力モードに*/
_ANSB0 = 0;/*RB0をデジタル入力として使用*/
_ANSB1 = 0;/*RB1をデジタル入力として使用*/
int data1, data2;
_TRISA0 = 1;/*RA0を入力モードに*/
_TRISB2 = 1;/*RB2を入力モードに*/
_PVCFG = 0;/*参照電源の +をVdd*/
_NVCFG =0;/*参照電源の-をVss*/
_ADCS =0;/*クロック分周なし*/
_SSRC =7;/*A/D変換自動開始*/
_SAMC =1;/*サンプルリング待ち時間*/
_FORM=0;/*符号なし整数データ*/
_CH0NA = 0;/*ー側 Vss 指定*/
_CSS0 = 1;/*AN0を登録*/
_CSS4 = 1;/*BN4を登録*/
_SMPI = 1;/*2チャンネル終了後に割愛*/
_CSCNA =1;/*チャンネルスキャン有効*/
_MODE12 = 1;/*12bit A/D変換を選択*/
_ADON =1;/*A/D変換機能有効化*/
_AD1IF = 0;/*割り込みフラグクリア*/
/*PWNモ−ド*/
/*右タイヤ*/
OC2RS =20000;/*PWM周期設定*/
OC2R=5000;/*デューティー比設定*/
OC2CON2bits.SYNCSEL = 0x1F;/*同期設定1*/
OC2CON2bits.OCTRIG = 0;/*同期設定2*/
OC2CON1bits.OCTSEL = 7;/*クロック選択*/
OC2CON1bits.OCM = 6;/*エッジアライン PWM選択*/
/*左タイヤ*/
OC3RS =20000;/*PWM周期設定*/
OC3R=9000;/*デューティー比設定*/
OC3CON2bits.SYNCSEL = 0x1F;/*同期設定1*/
OC3CON2bits.OCTRIG = 0;/*同期設定2*/
OC3CON1bits.OCTSEL = 7;/*クロック選択*/
OC3CON1bits.OCM = 6;/*エッジアライン PWM選択*/
while(1)
{
/*データの値を更新*/
left = _RB0;/*RB0データの読み出し 車体を後ろから見たときの左のラインセンサ*/
right = _RB1;/*RB1データの読み出し 車体を後ろから見たときの右ラインセンサ*/
_ASAM =1;/*連続変換開始*/
while(! _AD1IF);/*変換終了街待ち*/
_ASAM =0;/*連続変換終了*/
_AD1IF = 0;/*割り込みフラグクリア*/
data1 = ADC1BUF0;/*AN0変換データの読み出し 車体を後ろから見たときの左の近接セ
ンサ*/
data2 = ADC1BUF1;/*AN4変換データの読み出し 車体を後ろから見たときの右の近接セ
ンサ*/
/*光変調ICの反応による条件分け*/
if(left == 0 && right == 0)
{
/*左○○右*/
/*前進*/
OC2R=5000;/*デューティー比設定*/
OC3R=9000;/*デューティー比設定*/
_LATA7 = 0;/*ピン番号19(RA7)にデータ書き込み 0を出力*/
_LATA4 = 0; /*ピン番号12(RA4)にデータ書き込み 0を出力*/
if(data2 > 2500)
{
/*下がって右折*/
OC2R=5500;/*デューティー比設定*/
OC3R=7500;/*デューティー比設定*/
_LATA7 = 1;/*ピン番号19(RA7)にデータ書き込み 1を出力*/
_LATA4 = 1; /*ピン番号12(RA4)にデータ書き込み 1を出力*/
delay_ms(1500);/*1500msの間続ける*/
OC2R=9000;/*デューティー比設定*/
OC3R=9000;/*デューティー比設定*/
_LATA7 = 1;/*ピン番号19(RA7)にデータ書き込み 1を出力*/
_LATA4 = 0;/*ピン番号12(RA4)にデータ書き込み 0を出力*/
delay_ms(800);/*800msの間続ける*/
}
else if(data2 < 2500)
{
/*前進*/
OC2R=5000;/*デューティー比設定*/
OC3R=9000;/*デューティー比設定*/
_LATA7 = 0;/*ピン番号19(RA7)にデータ書き込み 0を出力*/
_LATA4 = 0; /*ピン番号12(RA4)にデータ書き込み 0を出力*/
if(data1 > 3400)
{
/*下がって右折*/
OC2R=5500;/*デューティー比設定*/
OC3R=7500;/*デューティー比設定*/
_LATA7 = 1;/*ピン番号19(RA7)にデータ書き込み 1を出力*/
_LATA4 = 1; /*ピン番号12(RA4)にデータ書き込み 1を出力*/
delay_ms(1000);/*1000msの間続ける*/
OC2R=9000;/*デューティー比設定*/
OC3R=9000;/*デューティー比設定*/
_LATA7 = 1;/*ピン番号19(RA7)にデータ書き込み 1を出力*/
_LATA4 = 0;/*ピン番号12(RA4)にデータ書き込み 0を出力*/
delay_ms(700);/*700msの間続ける*/
}
else if(data1 < 3400)
{
/*前進*/
OC2R=5000;/*デューティー比設定*/
OC3R=9000;/*デューティー比設定*/
_LATA7 = 0;/*ピン番号19(RA7)にデータ書き込み 0を出力*/
_LATA4 = 0; /*ピン番号12(RA4)にデータ書き込み 0を出力*/
}
}
}
else if(left == 1 && right == 0)
{
/*左●○右*/
/*下がって右折*/
OC2R=5500;
OC3R=7500;
_LATA7 = 1;/*ピン番号19(RA7)にデータ書き込み 1を出力*/
_LATA4 = 1;/*ピン番号12(RA4)にデータ書き込み 1を出力*/
delay_ms(1500);/*1000msの間続ける*/
OC2R=6000;
OC3R=9000;
_LATA7 = 1;/*ピン番号19(RA7)にデータ書き込み 1を出力*/
_LATA4 = 0;/*ピン番号12(RA4)にデータ書き込み 0を出力*/
delay_ms(1000);/*1000msの間続ける*/
}
else if(left == 1 && right == 1)
{
/*左●●右*/
/*下がって右折*/
OC2R=5500;
OC3R=7500;
_LATA7 = 0;/*ピン番号19(RA7)にデータ書き込み 0を出力*/
_LATA4 = 0;/*ピン番号12(RA4)にデータ書き込み 0を出力*/
delay_ms(1500);/*1500msの間続ける*/
OC2R=6000;
OC3R=9000;
_LATA7 = 1;/*ピン番号19(RA7)にデータ書き込み 1を出力*/
_LATA4 = 0;/*ピン番号12(RA4)にデータ書き込み 0を出力*/
delay_ms(1000);/*1000msの間続ける*/
}
else if(left == 0 && right == 1)
{
/*左○●右*/
/*下がって右折*/
OC2R=5500;
OC3R=7500;
_LATA7 = 1;/*ピン番号19(RA7)にデータ書き込み 1を出力*/
_LATA4 = 1;/*ピン番号12(RA4)にデータ書き込み 1を出力*/
delay_ms(1500);/*1500msの間続ける*/
OC2R=6000;
OC3R=9000;
_LATA7 = 1;/*ピン番号19(RA7)にデータ書き込み 1を出力*/
_LATA4 = 0;/*ピン番号12(RA4)にデータ書き込み 0を出力*/
delay_ms(900);/*9000msの間続ける*/
}
}
return 0;
}
ラインセンサはPICマイコンのCN4とCN5に繋ぐ。
近接センサはPICマイコンのCN2とCN6に繋ぐ。
モーターの制御はMOSFETを使用したhブリッジ回路でしている。
回路の入力の1つはそれぞれCN12とCN19に繋ぐ。ここのon/offの切り替えでモーターの正転/逆転を制御する。
もう一つはPWM信号で、デューティ比を変えることでモーターの回転速度を変化させる。