log編集中
Dependencies: mbed MPU6050_2 HMC5883L_2 SDFileSystem3
main.cpp@0:84ddd6d354e1, 2018-12-22 (annotated)
- Committer:
- HARUKIDELTA
- Date:
- Sat Dec 22 09:27:29 2018 +0000
- Revision:
- 0:84ddd6d354e1
- Child:
- 1:290e621741fd
a
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
HARUKIDELTA | 0:84ddd6d354e1 | 1 | #include "mbed.h" |
HARUKIDELTA | 0:84ddd6d354e1 | 2 | |
HARUKIDELTA | 0:84ddd6d354e1 | 3 | #define servo_NEUTRAL_R 1460 |
HARUKIDELTA | 0:84ddd6d354e1 | 4 | #define servo_NEUTRAL_L 1460 |
HARUKIDELTA | 0:84ddd6d354e1 | 5 | #define servo_FORWARD_R 1860 |
HARUKIDELTA | 0:84ddd6d354e1 | 6 | #define servo_FORWARD_L 1860 |
HARUKIDELTA | 0:84ddd6d354e1 | 7 | #define servo_back_R 1060 |
HARUKIDELTA | 0:84ddd6d354e1 | 8 | #define servo_back_L 1060 |
HARUKIDELTA | 0:84ddd6d354e1 | 9 | #define servo_slow_FORWARD_R 1560 |
HARUKIDELTA | 0:84ddd6d354e1 | 10 | #define servo_slow_FORWARD_L 1560 |
HARUKIDELTA | 0:84ddd6d354e1 | 11 | #define servo_slow_back_R 1360 |
HARUKIDELTA | 0:84ddd6d354e1 | 12 | #define servo_slow_back_L 1360 |
HARUKIDELTA | 0:84ddd6d354e1 | 13 | |
HARUKIDELTA | 0:84ddd6d354e1 | 14 | #define MOVE_NEUTRAL 0 |
HARUKIDELTA | 0:84ddd6d354e1 | 15 | #define MOVE_FORWARD 1 |
HARUKIDELTA | 0:84ddd6d354e1 | 16 | #define MOVE_LEFT 2 |
HARUKIDELTA | 0:84ddd6d354e1 | 17 | #define MOVE_RIGHT 3 |
HARUKIDELTA | 0:84ddd6d354e1 | 18 | #define MOVE_BACK 4 |
HARUKIDELTA | 0:84ddd6d354e1 | 19 | #define GOAL_FORWARD 5 //ゴール付近 ゆっくり |
HARUKIDELTA | 0:84ddd6d354e1 | 20 | #define GOAL_LEFT 6 |
HARUKIDELTA | 0:84ddd6d354e1 | 21 | #define GOAL_RIGHT 7 |
HARUKIDELTA | 0:84ddd6d354e1 | 22 | #define MAX_FORWARD 8 //はやい 姿勢修正用 |
HARUKIDELTA | 0:84ddd6d354e1 | 23 | #define MAX_BACK 9 |
HARUKIDELTA | 0:84ddd6d354e1 | 24 | |
HARUKIDELTA | 0:84ddd6d354e1 | 25 | PwmOut servoR(PC_6); |
HARUKIDELTA | 0:84ddd6d354e1 | 26 | PwmOut servoL(PC_7); |
HARUKIDELTA | 0:84ddd6d354e1 | 27 | |
HARUKIDELTA | 0:84ddd6d354e1 | 28 | void MoveCansat(int code); |
HARUKIDELTA | 0:84ddd6d354e1 | 29 | |
HARUKIDELTA | 0:84ddd6d354e1 | 30 | int main() { |
HARUKIDELTA | 0:84ddd6d354e1 | 31 | while(1) { |
HARUKIDELTA | 0:84ddd6d354e1 | 32 | //printf("Hello World!!"); |
HARUKIDELTA | 0:84ddd6d354e1 | 33 | MoveCansat(MOVE_NEUTRAL); |
HARUKIDELTA | 0:84ddd6d354e1 | 34 | printf("STOP\r\n"); |
HARUKIDELTA | 0:84ddd6d354e1 | 35 | wait(2); |
HARUKIDELTA | 0:84ddd6d354e1 | 36 | MoveCansat(MOVE_FORWARD); |
HARUKIDELTA | 0:84ddd6d354e1 | 37 | printf("Move Forward\r\n"); |
HARUKIDELTA | 0:84ddd6d354e1 | 38 | wait(2); |
HARUKIDELTA | 0:84ddd6d354e1 | 39 | MoveCansat(MOVE_RIGHT); |
HARUKIDELTA | 0:84ddd6d354e1 | 40 | printf("Move right\r\n"); |
HARUKIDELTA | 0:84ddd6d354e1 | 41 | wait(2); |
HARUKIDELTA | 0:84ddd6d354e1 | 42 | MoveCansat(MOVE_LEFT); |
HARUKIDELTA | 0:84ddd6d354e1 | 43 | printf("Move left\r\n"); |
HARUKIDELTA | 0:84ddd6d354e1 | 44 | wait(2); |
HARUKIDELTA | 0:84ddd6d354e1 | 45 | } |
HARUKIDELTA | 0:84ddd6d354e1 | 46 | } |
HARUKIDELTA | 0:84ddd6d354e1 | 47 | |
HARUKIDELTA | 0:84ddd6d354e1 | 48 | void MoveCansat(int code) |
HARUKIDELTA | 0:84ddd6d354e1 | 49 | { |
HARUKIDELTA | 0:84ddd6d354e1 | 50 | switch(code){ |
HARUKIDELTA | 0:84ddd6d354e1 | 51 | case 0: //MOVE_NEUTRAL |
HARUKIDELTA | 0:84ddd6d354e1 | 52 | servoR.pulsewidth_us(servo_NEUTRAL_R); |
HARUKIDELTA | 0:84ddd6d354e1 | 53 | servoL.pulsewidth_us(servo_NEUTRAL_L); |
HARUKIDELTA | 0:84ddd6d354e1 | 54 | break; |
HARUKIDELTA | 0:84ddd6d354e1 | 55 | case 1: //MOVE_FORWARD |
HARUKIDELTA | 0:84ddd6d354e1 | 56 | servoR.pulsewidth_us(servo_FORWARD_R); |
HARUKIDELTA | 0:84ddd6d354e1 | 57 | servoL.pulsewidth_us(servo_FORWARD_L); |
HARUKIDELTA | 0:84ddd6d354e1 | 58 | break; |
HARUKIDELTA | 0:84ddd6d354e1 | 59 | case 2: //MOVE_LEFT |
HARUKIDELTA | 0:84ddd6d354e1 | 60 | servoR.pulsewidth_us(servo_slow_FORWARD_R); |
HARUKIDELTA | 0:84ddd6d354e1 | 61 | servoL.pulsewidth_us(servo_slow_back_L); |
HARUKIDELTA | 0:84ddd6d354e1 | 62 | case 3: //MOVE_RIGHT |
HARUKIDELTA | 0:84ddd6d354e1 | 63 | servoR.pulsewidth_us(servo_slow_back_R); |
HARUKIDELTA | 0:84ddd6d354e1 | 64 | servoL.pulsewidth_us(servo_slow_FORWARD_L); |
HARUKIDELTA | 0:84ddd6d354e1 | 65 | break; |
HARUKIDELTA | 0:84ddd6d354e1 | 66 | case 4: //MOVE_BACK |
HARUKIDELTA | 0:84ddd6d354e1 | 67 | servoR.pulsewidth_us(servo_back_R); |
HARUKIDELTA | 0:84ddd6d354e1 | 68 | servoL.pulsewidth_us(servo_back_L); |
HARUKIDELTA | 0:84ddd6d354e1 | 69 | break; |
HARUKIDELTA | 0:84ddd6d354e1 | 70 | case 5: //GOAL_FORWARD |
HARUKIDELTA | 0:84ddd6d354e1 | 71 | servoR.pulsewidth_us(servo_slow_FORWARD_R); |
HARUKIDELTA | 0:84ddd6d354e1 | 72 | servoL.pulsewidth_us(servo_slow_FORWARD_L); |
HARUKIDELTA | 0:84ddd6d354e1 | 73 | break; |
HARUKIDELTA | 0:84ddd6d354e1 | 74 | case 6: //GOAL_LEFT |
HARUKIDELTA | 0:84ddd6d354e1 | 75 | servoR.pulsewidth_us(servo_slow_FORWARD_R); |
HARUKIDELTA | 0:84ddd6d354e1 | 76 | servoL.pulsewidth_us(servo_slow_back_L); |
HARUKIDELTA | 0:84ddd6d354e1 | 77 | break; |
HARUKIDELTA | 0:84ddd6d354e1 | 78 | case 7: //GOAL_RIGHT |
HARUKIDELTA | 0:84ddd6d354e1 | 79 | servoR.pulsewidth_us(servo_slow_back_R); |
HARUKIDELTA | 0:84ddd6d354e1 | 80 | servoL.pulsewidth_us(servo_slow_FORWARD_L); |
HARUKIDELTA | 0:84ddd6d354e1 | 81 | break; |
HARUKIDELTA | 0:84ddd6d354e1 | 82 | case 8: //MAX_FORWARD |
HARUKIDELTA | 0:84ddd6d354e1 | 83 | servoR.pulsewidth_us(servo_FORWARD_R); |
HARUKIDELTA | 0:84ddd6d354e1 | 84 | servoL.pulsewidth_us(servo_FORWARD_L); |
HARUKIDELTA | 0:84ddd6d354e1 | 85 | break; |
HARUKIDELTA | 0:84ddd6d354e1 | 86 | case 9: //MAX_BACK |
HARUKIDELTA | 0:84ddd6d354e1 | 87 | servoR.pulsewidth_us(servo_back_R); |
HARUKIDELTA | 0:84ddd6d354e1 | 88 | servoL.pulsewidth_us(servo_back_L); |
HARUKIDELTA | 0:84ddd6d354e1 | 89 | break; |
HARUKIDELTA | 0:84ddd6d354e1 | 90 | } |
HARUKIDELTA | 0:84ddd6d354e1 | 91 | return; |
HARUKIDELTA | 0:84ddd6d354e1 | 92 | } |