C言語 etロボ 走って右方向に行く

詳細

ファイル:Etロボのまっすぐ行って戻って左旋回.mp4

etロボがまっすぐ走ってUターンしてきて左に旋回して動くだけのプログラムです.

コード

/* helloworld.c for TOPPERS/ATK(OSEK) */
#include "kernel.h"
#include "kernel_id.h"
#include "stdio.h"
#include "stdlib.h"
#include "math.h"
#include "ecrobot_interface.h"

DeclareTask(Task1);				/* Task1を宣言 */

void main_task(void);
void go(void); //両タイヤの状態確認と開始
void back(void);
void stop(void);
void left_go(void);
void rigth_go(void);
static float save = 0.0f;
static int state;
static int ok = 0;
static int mb = 1;
static float v = 0.0f;

float angle_M(float TRED_WIDTH,float TYRE_DIAMTER,float encoder2){
    v += (180.0f/((((TRED_WIDTH*2.0f)*M_PI)*180.0f/360.0f)/(TYRE_DIAMTER*M_PI)))*(encoder2/360.0f);
    return v;
}

void ecrobot_device_initialize(){		/* OSEK起動時の処理（モータ停止）*/
	nxt_motor_set_speed(NXT_PORT_B,0,1);
	nxt_motor_set_speed(NXT_PORT_C,0,1);
	nxt_motor_set_count(NXT_PORT_C,0);
	nxt_motor_set_count(NXT_PORT_B,0);
}

void ecrobot_device_terminate(){		/* OSEK終了時の処理（モータ停止）*/
	nxt_motor_set_speed(NXT_PORT_B,0,0);
	nxt_motor_set_speed(NXT_PORT_C,0,0);
}

/* nxtOSEK hook to be invoked from an ISR in category 2 */
void user_1ms_isr_type2(void){ /* do nothing */ }

TASK(OSEK_Task_Background)
{
    ecrobot_device_initialize();
		while(1){
            main_task(); //ある一定間で呼び出されるタスク
            /* button_task();*/
            if(ok >= 1){ // 脱出
                break;
            }
        }
    ecrobot_device_terminate();
	TerminateTask();
}
void main_task(void){

    //必要な状態を数学に割り当てる
    #define STATE_STOPPING 0 //止まっている状態
    #define STATE_WAIT_BTN 1 //ボタンの押され待ち状態
    #define STATE_RUNNING 2  //走っている状態
    #define STATE_WAIT_1000 3 //
    #define STATE_BACK 4 //
    #define STATE_END 5 //
    #define STATE_TURN 6 //
	#define STATE_FIN 7

    #define TYRE_DIAMTER 56.0f //mm
    #define DISTANCE 1000.0f //mm
    #define TRED_WIDTH 112.0f //

    float mm_per_rotate = TYRE_DIAMTER * M_PI; //自分の状態を保持しておく変数
    float target_rotate = DISTANCE / mm_per_rotate; //1m進む時のタイヤ回転数
    int target_degree =  target_rotate * 360; //1m進むときの回転数


    static float encoder = 0.0f; //モータのエンコーダ（回転角度）の値
    static float anglePerProcess = 0.0f;
    static int process = 0;
    if(mb <= 1){
    state = STATE_RUNNING; //自分の状態を保持しておく変数
    go();
    mb = 2;
    }

    switch(state){
    ecrobot_device_initialize();
        case STATE_RUNNING:
            encoder += nxt_motor_get_count(NXT_PORT_B);
        	nxt_motor_set_count(NXT_PORT_C,20);
        	nxt_motor_set_count(NXT_PORT_B,20);
            if(encoder > (target_degree+20)){
            	ecrobot_device_initialize();
                systick_wait_ms(10);
                save = encoder;
                encoder = 0.0f;
                left_go(); //回転の為

                state = STATE_TURN;
            }
            systick_wait_ms(1000);
            break;

        case STATE_TURN: //ここさえわかればイケる
            process += 1;
            anglePerProcess += angle_M(TRED_WIDTH,TYRE_DIAMTER,nxt_motor_get_count(NXT_PORT_B));
        	nxt_motor_set_count(NXT_PORT_C,20);
        	nxt_motor_set_count(NXT_PORT_B,20);
            if(anglePerProcess >= 183.5f){
            	ecrobot_device_initialize();
                systick_wait_ms(50);
                go();
                encoder = 0.0f;
                state = STATE_BACK;
            }
            break;

        case STATE_BACK:
            encoder += nxt_motor_get_count(NXT_PORT_B);
            if(encoder > (target_degree+210)){
            	ecrobot_device_initialize();
                stop();
                //systick_wait_ms(50);
                state = STATE_END;
            }
            systick_wait_ms(1000);
            break;

        case STATE_END:
        	ecrobot_device_initialize();
        	stop();
        	state = STATE_FIN;
            break;

        case STATE_FIN:
        	ok = 1;
        	break;

    }

}

/*void button_task(void){

    //terminateTask();
}*/

void go(void){
    nxt_motor_set_speed(NXT_PORT_B,98,0);
    nxt_motor_set_speed(NXT_PORT_C,100,0);
}

void back(void){
    nxt_motor_set_speed(NXT_PORT_B,-100,0);
    nxt_motor_set_speed(NXT_PORT_C,-100,0);
}

void left_go(void){
    nxt_motor_set_speed(NXT_PORT_B,100,0);
    nxt_motor_set_speed(NXT_PORT_C,0,1);
}

void rigth_go(void){
    nxt_motor_set_speed(NXT_PORT_B,0,1);
    nxt_motor_set_speed(NXT_PORT_C,100,0);
}

void stop(void){
    nxt_motor_set_speed(NXT_PORT_B,0,0);
    nxt_motor_set_speed(NXT_PORT_C,0,0);
}