- 追加された行はこの色です。
- 削除された行はこの色です。
[[2012a/MemberOnly/進行状況A]]
[[ロボティクス入門ゼミ]]
[[ロボティクス入門ゼミ]]
[[2012a/A5/Liwon7/M3]]
#contents
*アーム部分 [#x4bcc592]
&br; 最初のアームがうまくいかなかったので、シンプルにしてできるだけ軽くした。
&br; ・旧
#ref(2012a/A5/ITY/M3/CIMG2288.JPG,wrap)
#ref(2012a/A5/ITY/M3/CIMG2289.JPG,wrap)
#ref(2012a/A5/ITY/M3/CIMG2294.JPG,wrap)
#ref(2012a/A5/ITY/M3/CIMG2291.JPG,wrap)
&br; ・新
#ref(2012a/A5/ITY/M3/CIMG2298.JPG,wrap)
#ref(2012a/A5/ITY/M3/CIMG2301.JPG,wrap)
#ref(2012a/A5/ITY/M3/CIMG2296.JPG,wrap)
*プログラム [#vde8e72f]
**フローチャート [#j3b4300d]
・缶を探すプログラム
&br; ・新
#ref(2012a/A5/ITY/M3/無題1.png,wrap)
左回転させて、超音波センサの値が、最初に動かした時に取得した値より小さければ、右旋回させて前進させる。
&br; ・旧
#ref(2012a/A5/ITY/M3/無題.png,wrap)
左右に回転させて、一番小さい値のところで前進させる。
**プログラム(新) [#pcc91387]
/* 2012/7/27 ITY 借用 Haxa */
/* 2012a_robo robokon12 */
/*-------------------------------------------------------------------
別名定義
-------------------------------------------------------------------*/
#define PWM 80 // PWM(力の大きさ)
#define PWM_D 15 // PWM(力の大きさ)
#define S_BR 40 // 黒線のしきい値
#define S_WH 60 // 白線のしきい値
#define DIF 6 // 差
#define ON 1 // オン
#define OFF 0 // オフ
#define EN_D 40 // 円の直径
#define TY_B 150 // タイヤベース
#define TY_D 55 // タイヤの直径
#define PAI 3.14 // 円周率3.14
#define FF 1 // 前進
#define FR 2 // 右旋回
#define RF 3 // 左旋回
#define RR 4 // 後進
#define BB 5 // ブレーキ
/*-------------------------------------------------------------------
関数宣言
-------------------------------------------------------------------*/
void LCD_D(); // LCDの表示
void TRACE(); // ライントレース
void START(); // スタート
void mt_ct(int mt_ct,int ang_d);
/*-------------------------------------------------------------------
グローバル変数
-------------------------------------------------------------------*/
int s_lowse1,s_lowse2,s_light1,s_light2; // 変数宣言
/*-------------------------------------------------------------------
メインルーチン
-------------------------------------------------------------------*/
task main()
{
SetSensorLight(IN_4); // 光センサの宣言
SetSensorLight(IN_3); // 光センサの宣言
SetSensorTouch(IN_2); // タッチセンサの宣言
SetSensorLowspeed(IN_1); // 超音波センサ
int speed,diff,nairin,in; // 変数宣言
float p; // フロートで宣言
int temp; // 一時退避
int cnt=0,cnt_t0=0,cnt_t1=0,mode=0; // 変数の初期化
int temp0=50,temp1=50,cnt_t=0; // 変数の初期化
int time=15,pw=80; // 時間15ms_PMW80%
temp = SensorUS(IN_1); // 初期の値を取得
while(true){ // 繰り返す
s_lowse1 = SensorUS(IN_1); // 変数の代入
s_lowse2 = Sensor(IN_2); // 変数の代入
s_light1 = Sensor(IN_3); // 変数の代入
s_light2 = Sensor(IN_4); // 変数の代入
SendRemoteNumber(MAILBOX2,true,2); // 2を送る(掴む動作の開始)
while(true){
ReceiveRemoteNumber(MAILBOX3,true,in);//メッセージを受けとる
if(in==3){ // 3が来るまでまで無限ループ
break; // 抜ける
}
SendRemoteNumber(MAILBOX2,true,1); // 1を送る(掴む動作の停止)
}
// START(); // 開始
mt_ct(FF,100); // 前進
mt_ct(FF,200); // 前進
mt_ct(RF,93); // 90度左旋回
mt_ct(FF,640); // 前進
p = 1; // pに1を代入
while(SensorUS(IN_1) > 8){ // 超音波センサの値が8以上の時
mt_ct(RF,(10*p+6)); // 左旋回
if((temp+3) > SensorUS(IN_1)){ // センサが初期の値より小さい時
p = p * 0.6; // 回転角を小さくする
RotateMotor(OUT_AC,pw,60);// 前進
mt_ct(FR,(15*p)); // 右旋回
}
else if(p < 1){ // pが1より小さい時(反応があった時)
mt_ct(FR,15); // 右旋回
p = 1; // pを初期化
}
}
OnFwdSync(OUT_AC,80,0); // 前進する
while((Sensor(IN_2) == OFF)){}; // タッチセンサに反応があるまで前進
Off(OUT_AC); // モータの停止
SendRemoteNumber(MAILBOX2,true,2); // 2を送る(缶を降ろす動作)
Wait(6000); // 6秒間待つ
OnRevSync(OUT_AC,100,0); // 1秒間後退する
Wait(1000);
mt_ct(RF,200); // 90度左旋回
mt_ct(FF,600); // 前進
p = 1; // pの初期化
while(SensorUS(IN_1) > 6){ // 超音波センサの値が6以上の時
mt_ct(RF,10); // 左旋回
if((temp+5) > SensorUS(IN_1)){ // センサが初期の値より小さい時
RotateMotor(OUT_AC,pw,200);// 前進
mt_ct(FR,(10*p*10)/10); // 右旋回
p = p * 0.6; // 回転角を小さくする
}
else p = 1; // pの 初期化
}
OnFwdSync(OUT_AC,80,0); // 前進する
while((Sensor(IN_2) == OFF)){}; // タッチセンサに反応があるまで前進
Off(OUT_AC); // 停止
while(true){}; // 停止
// ResetScreen(); // LCDの初期化
// LCD_D(); // LCDの表示
// TRACE(); // トレースの開始
}
}
/*-------------------------------------------------------------------
サブルーチン
-------------------------------------------------------------------*/
/* 進む距離回転する角度計算用関数 */
void mt_ct(int mt_ct,int ang_d)
{
int r,d,g,mt;
int iNum1=0,iNum2=0;
r = TY_D * PAI; //タイヤの円周
// 進む距離計算
if(mt_ct == FR || mt_ct == RF){
d = TY_B * PAI * ang_d / 360;
}
else{
d = ang_d;
}
mt = 360 * d / r; //進む距離からパルス数計算
switch(mt_ct){
case FF: //前進
OnFwdSyncEx(OUT_AC,PWM,0,RESET_ALL);
break;
case FR: //右旋回
OnFwdSyncEx(OUT_AC,PWM,100,RESET_ALL);
break;
case RF: //左旋回
OnFwdSyncEx(OUT_AC,PWM,-100,RESET_ALL);
break;
case RR: //後進
OnRevSyncEx(OUT_AC,PWM,0,RESET_ALL);
break;
case BB: //ブレーキ
Off(OUT_AC);
break;
default:
Off(OUT_AC);
break;
}
while(abs(iNum1) < mt || abs(iNum2) < mt){//左右のパルス数が目標値になるまで待つ
iNum1 = MotorTachoCount(OUT_A);
iNum2 = MotorTachoCount(OUT_C);
}
Off(OUT_AC); //両輪停止
Wait(500); //500ms待つ
}
/*-------------------------------------------------------------------
進む距離回転する角度計算用関数 はHexaさんより借用
-------------------------------------------------------------------*/
void LCD_D(){ // LCDの表示
while(true){
ResetScreen();
NumOut(10,48,SensorUS(IN_1),0); // 超音波センサの値を表示
NumOut(10,40,SensorUS(IN_2),0); // 超音波センサの値を表示
NumOut(10,32,Sensor(IN_3),0); // 光センサの値を表示
NumOut(10,24,Sensor(IN_4),0); // 光センサの値を表示
Wait(100);
}
}
void START(){ // 後退、前進、回転を繰り返す
int i; // 変数宣言
for(i=0;i<3;i++){ // 3回繰り返す
OnRev(OUT_AC,PWM); // 後退する
Wait(100); // 0.1秒待つ
while((s_light1 > S_WH) && (s_light2 > S_WH)){
OnFwd(OUT_AC,PWM); // 前進
}
if(s_light1 < S_BR){
Off(OUT_C); // モータCの停止
while(s_light2 < S_BR)OnFwd(OUT_A,PWM);
}
else if(s_light2 < S_BR){
Off(OUT_A); // モータAの停止
while(s_light1 < S_BR)OnFwd(OUT_C,PWM);
}
Wait(100); // 1秒間待つ
}
}
void TRACE(){ // 黒線のトレース
int nairin,mode; // 変数宣言
mode=0; // 変数の初期化
// ResetTachoCount(OUT_AB); // タコメータのカウント
nairin = ((PWM*(EN_D-TY_B)*10)/(EN_D+TY_B)/10);
// 内輪差の計算
if((s_light1 > S_WH) && (s_light2 > S_WH)){
OnFwd(OUT_A,nairin); // 前進
OnFwd(OUT_C,PWM); // 前進
}
else {
if(s_light1 < S_BR){
OnFwd(OUT_A,nairin*0.8);// 前進
OnFwd(OUT_C,PWM); // 前進
if(mode/2 == 1){
mode++; // モードの増加
}
}
else if(s_light2 < S_BR){
OnFwd(OUT_A,PWM); // 前進
OnFwd(OUT_C,nairin*0.8);// 前進
if(mode/2 == 0){
mode++;
}
}
}
}
**缶を探すプログラム(旧) [#y922efc4]
・上手く動作しなかったプログラム
/* 2012/7/26 ITY */
/* 2012a_robo robokon8 */
while(SensorUS(IN_1) > 6){ // 超音波センサの値が6以上の時
if(mode == 0){ // モードが0の時
cnt++; // カウントアップ
if(cnt < time){ // カウントがタイムより小さい時
if(temp0 > SensorUS(IN_1)){ // 超音波センサの値が前の値より小さい時
temp0 = SensorUS(IN_1); // tempにセンサの値を代入
cnt_t0 = cnt; // cnt_t0にcnt(時間)を代入
}
OnFwd(OUT_A,pw); // モータAを正転(右回転)
OnRev(OUT_C,pw); // モータCを後転(右回転)
}
else if(cnt < time*2){ // カウントがタイムより小さい時
OnFwd(OUT_C,pw); // モータCを正転(左回転)
OnRev(OUT_A,pw); // モータAを後転(左回転)
}
}
else if(mode == 1){ // モードが1の時
cnt++; // カウントアップ
if(cnt < time){ // カウントがタイムより小さい時
if(temp1 > SensorUS(IN_1)){ // 超音波センサの値が前の値より小さい時
temp1 = SensorUS(IN_1); // tempにセンサの値を代入
cnt_t1 = cnt; // cnt_t1にcnt(時間)を代入
}
OnFwd(OUT_C,pw); // モータCを正転(左回転)
OnRev(OUT_A,pw); // モータAを後転(左回転)
}
else if(cnt < time*2){ //
OnFwd(OUT_A,pw); // モータAを正転(右回転)
OnRev(OUT_C,pw); // モータCを後転(右回転)
}
}
else if(mode == 2){ // モードが2の時
if(temp0 > temp1){ // temp0がtemp1より大きい時
OnFwd(OUT_C,pw); // モータCを正転(左回転)
OnRev(OUT_A,pw); // モータAを後転(左回転)
cnt_t = cnt_t1; // cnt_tに代入
}
else if(temp0 < temp1){ // temp1がtemp0より大きい時
OnFwd(OUT_A,pw); // モータAを正転(右回転)
OnRev(OUT_C,pw); // モータCを後転(右回転)
cnt_t = cnt_t0; // cnt_tに代入
}
if((temp0 > 20) && (temp1 > 20)){ // temp1、temp0ともに20より大きい時
cnt_t = 0; // cnt_tを初期化
}
while(cnt < cnt_t){ // cntがcnt_tより小さい時
cnt++; // カウントアップ
}
OnFwd(OUT_AC,pw); // 0.5秒間前進
Wait(500); // 初期化
mode = 0; // 初期化
cnt = 0; // 初期化
cnt_t = 0; // 初期化
cnt_t0 = 0; // 初期化
cnt_t1 = 0; // 初期化
temp0 = 50; // 初期化
temp1 = 50; // 初期化
}
if(cnt >= time*2){ // cntがtimeに2倍より大きい時
if(mode == 0){ // モードが0の時
mode = 1; // モードを1にする
}
else if(mode == 1){ // モードが1の時
mode = 2; // モードを2にする
}
cnt = 0; // 初期化
}
}
*感想 [#y90c61e5]
&br; ・なかなか思うように車体ができず、一個積み上げるのを狙いとしたロボットになってしまった。プログラムの方も詰めが甘く、車体的にタッチセンサを押したら絶対に置くことができる車体だったので、もったいなかった。
&br; ・ロボットを制御することは難しいと改めて感じ、ロボットが必ず思い通りに動いたり、絶対に安全といい切ることはできないと感じた。