sdカードに焦点調整を追加

Dependencies:   mbed MPU6050_2 HMC5883L_2 SDFileSystem3

main.cpp

Committer:
HARUKIDELTA
Date:
6 months ago
Revision:
0:84ddd6d354e1
Child:
1:290e621741fd

File content as of revision 0:84ddd6d354e1:

#include "mbed.h"

#define servo_NEUTRAL_R    1460
#define servo_NEUTRAL_L    1460
#define servo_FORWARD_R    1860
#define servo_FORWARD_L    1860
#define servo_back_R       1060
#define servo_back_L       1060
#define servo_slow_FORWARD_R    1560
#define servo_slow_FORWARD_L    1560
#define servo_slow_back_R       1360
#define servo_slow_back_L       1360

#define MOVE_NEUTRAL    0
#define MOVE_FORWARD    1   
#define MOVE_LEFT       2
#define MOVE_RIGHT      3
#define MOVE_BACK       4
#define GOAL_FORWARD    5          //ゴール付近 ゆっくり
#define GOAL_LEFT       6
#define GOAL_RIGHT      7
#define MAX_FORWARD     8          //はやい 姿勢修正用
#define MAX_BACK        9

PwmOut servoR(PC_6);
PwmOut servoL(PC_7);

void MoveCansat(int code);

int main() {
    while(1) {
        //printf("Hello World!!");
        MoveCansat(MOVE_NEUTRAL);
        printf("STOP\r\n");
        wait(2);
        MoveCansat(MOVE_FORWARD);
        printf("Move Forward\r\n");
        wait(2);
        MoveCansat(MOVE_RIGHT);
        printf("Move right\r\n");
        wait(2);
        MoveCansat(MOVE_LEFT);
        printf("Move left\r\n");
        wait(2);
        }
}

void MoveCansat(int code)
{
    switch(code){
        case 0: //MOVE_NEUTRAL 
            servoR.pulsewidth_us(servo_NEUTRAL_R);
            servoL.pulsewidth_us(servo_NEUTRAL_L);
            break;
        case 1: //MOVE_FORWARD
            servoR.pulsewidth_us(servo_FORWARD_R);
            servoL.pulsewidth_us(servo_FORWARD_L);
            break;
        case 2: //MOVE_LEFT 
            servoR.pulsewidth_us(servo_slow_FORWARD_R);
            servoL.pulsewidth_us(servo_slow_back_L);
        case 3: //MOVE_RIGHT 
            servoR.pulsewidth_us(servo_slow_back_R);
            servoL.pulsewidth_us(servo_slow_FORWARD_L);
            break;
        case 4: //MOVE_BACK 
            servoR.pulsewidth_us(servo_back_R);
            servoL.pulsewidth_us(servo_back_L);
            break;
        case 5: //GOAL_FORWARD
            servoR.pulsewidth_us(servo_slow_FORWARD_R);
            servoL.pulsewidth_us(servo_slow_FORWARD_L);
            break;
        case 6: //GOAL_LEFT 
            servoR.pulsewidth_us(servo_slow_FORWARD_R);
            servoL.pulsewidth_us(servo_slow_back_L);
            break;
        case 7: //GOAL_RIGHT 
            servoR.pulsewidth_us(servo_slow_back_R);
            servoL.pulsewidth_us(servo_slow_FORWARD_L);
            break;
        case 8: //MAX_FORWARD 
            servoR.pulsewidth_us(servo_FORWARD_R);
            servoL.pulsewidth_us(servo_FORWARD_L);
            break;
        case 9: //MAX_BACK 
            servoR.pulsewidth_us(servo_back_R);
            servoL.pulsewidth_us(servo_back_L);
            break;
    }
    return;
}