ロボット

// ////////////////////////////////////////////////////////////////////
// filename : 07.c
// programmer : MURAKAMI & MOTOKI
// created on : '09 09.07
// ////////////////////////////////////////////////////////////////////

#include <p30f6014A.h>
#include <motor_led/e_epuck_ports.h>
#include <motor_led/e_init_port.h>
#include <motor_led/e_motors.h>
#include <uart/e_uart_char.h>
#include <stdio.h>
#include <stdlib.h>

#define COMSTRLEN 32
#define MAXCOM 2

#define ERROR -1
#define AUTO 0
#define MANUAL 1
#define FORWARD 2
#define BACKWARD 3
#define TURNLEFT 4
#define TURNRIGHT 5

char *COM[]={"auto", "manual"};

// ////////////////////////////////////////////////////////////////////
int strlength(char str[])
{
int i;

i=0;
while (str[i]!='\0') i++;
return i;
}

//一致したら1を返す
int strcomp(char str1[], char str2[])
{
int i, eq;

i=0;
eq=0;
do {
if (str1[i]=='\0' && str2[i]=='\0') { eq=1; break; }
if (str1[i]!=str2[i]) break;
i++;
} while (1);
return eq;
}

void itoa(int dt, int bs, char str[])
{
int i, n, dt2;

i=0;
if (dt==0) { str[0]='0'; n=1; }
else {
if (dt<0) { dt=-dt; str[0]='-'; i=1; }
n=i; dt2=dt;
while(dt2>0) {dt2=dt2/bs; n++; }
i=1;
while(dt>0) {
str[n-i]=dt%bs+'0';
dt=dt/bs;
i++;
}
}
str[n]='\0';
}

// ////////////////////////////////////////////////////////////////////
char get1char(void)
{
char dt;
int i;

dt=0x00;
for (i=0 ; i<5000 ; i++) {
if (!e_uart1_sending() && e_ischar_uart1()) {
e_getchar_uart1(&dt);
break;
}
}
if (dt==0x00)for(i=0 ; i<5000 ; i++) asm("nop");
return dt;
}

void getstr(char comstr[])
{
char dt;
int i;
int j;
int flag = 0;

i=0;
j=0;
do{
for( j = 0 ; j < 50 ; j++ ){
dt=get1char();
if (dt!=0) {
flag = 1;
if (dt==';' || i+1>=COMSTRLEN) { //行頭復帰 OR 文字列数オーバー
comstr[i]='\0';
flag = 2;
break;
}
else comstr[i]=dt;
i++;
}
}
if( flag == 0 ){
comstr[0] = '\0';
break;
}
if( flag == 2 ){
break;
}

} while (1);
}

void put1char(char dt)
{
e_send_uart1_char(&dt, 1);
}

void putstr(char str[])
{
e_send_uart1_char(str, strlength(str));
}

int get_command(char comstr[])
{
int com;

for (com=0 ; com<MAXCOM ; com++) {
if (strcomp(comstr, COM[com])==1){
break; //命令一致
}
}
if (com>=MAXCOM){ //命令不一致
com=-1;
}
return com;
}

/////////////////////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////////////////////
//関数名 : check
//機能 : モード切替確認
//引数 : なし
//返り値 : モード
/////////////////////////////////////////////////////////////////////////////
int check( void )
{
int i = 0;
int j = 0;
char comstr[COMSTRLEN]; //入力命令
int cnum = ERROR; //命令番号
char numstr[8]; //命令番号

putstr("check\r\n");

getstr(comstr); //命令取得
cnum = get_command(comstr); //既存命令参照
itoa(cnum, 10, numstr); //整数→文字列

for( i = 0 ; i <= 400 ; i++ ){
for( j = 0 ; j <= 400 ; j++ ){
}
}
putstr("3\r\n");

return cnum; //自動・手動・エラー（継続）
}
/////////////////////////////////////////////////////////////////////////////
//関数名 : movement
//機能 : 基本動作（前進、後退、左右回転）
//引数 : operation(動作命令　前進:2 後退:3 左回転:4 右回転:5)
// distance(移動距離　step)
//返り値 : なし
/////////////////////////////////////////////////////////////////////////////
void movement( int operation , int distance )
{
int lSpeed = 0; //左モータスピード
int rSpeed = 0; //右モータスピード

putstr("movement\r\n");
switch( operation ){

case FORWARD:

lSpeed = 1000;
rSpeed = 1000;
break;

case BACKWARD:

lSpeed = -1000;
rSpeed = -1000;
break;

case TURNLEFT:

lSpeed = -1000;
rSpeed = 1000;
break;

case TURNRIGHT:

lSpeed = 1000;
rSpeed = -1000;
break;

default:

break;

}


e_set_steps_left( 0 ); //ステップ数初期化
e_set_steps_right( 0 );

e_set_speed_left( lSpeed ); //モータ速度設定
e_set_speed_right( rSpeed );

while( 1 ){ //指定距離分動作させる

if( abs( e_get_steps_left() ) == distance ){
break;
}

}
}

/////////////////////////////////////////////////////////////////////////////
//関数名 : autoMode
//機能 : 競技用動作
//引数 : なし
//返り値 : モード
/////////////////////////////////////////////////////////////////////////////
int autoMode( void )
{
putstr("auto\r\n");
/////////////////////////////競技用動作入力領域//////////////////////
////////////////////////////ここを調節してね！//////////////////////
movement( FORWARD , 17500 );
if( check() == MANUAL ) return MANUAL;
movement( BACKWARD , 2000 );
if( check() == MANUAL ) return MANUAL;
movement( TURNLEFT , 900 );
if( check() == MANUAL ) return MANUAL;

////////////////////////////////////////////////////////////////////

return AUTO;

}

/////////////////////////////////////////////////////////////////////////////
//関数名 : manualMode
//機能 : モータ停止
//引数 : なし
//返り値 : モード
/////////////////////////////////////////////////////////////////////////////
int manualMode( void )
{
putstr("manual\r\n");
e_set_speed_left( 0 );
e_set_speed_right( 0 );

if( check() == AUTO ){
return AUTO;
}
else{
return MANUAL;
}
}
/////////////////////////////////////////////////////////////////////////////
//関数名 : main
//機能 : ロボット製作実習最終課題
//引数 : なし
//返り値 : なし
///////////////////////////////////////////////////////////////////////////////
void main(void)
{

int flag = MANUAL; //自動・手動切替

e_init_port();
e_init_prox();
e_init_uart1();
e_init_motors();
e_send_uart1_char("Input(send) the command to e-puck.\r\n", 36);

while(1) {

putstr("main\r\n");

if( flag == MANUAL ){
flag = manualMode();
}
else if( flag == AUTO ){
flag = autoMode();
}

}
}