CanSat-C 2021 / Mbed 2 deprecated MoveTest

test

Dependencies:   mbed TB6612FNG HMC6352

main.cpp@0:4c10d798d1bb, 2021-10-13 (annotated)

Committer:
ushiroji
Date:
Wed Oct 13 10:49:38 2021 +0000
Revision:
0:4c10d798d1bb
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ushiroji 0:4c10d798d1bb 1 #include "mbed.h"
ushiroji 0:4c10d798d1bb 2 #include "TB6612.h"
ushiroji 0:4c10d798d1bb 3 #include "HMC6352.h"
ushiroji 0:4c10d798d1bb 4 #include "us015.h"
ushiroji 0:4c10d798d1bb 5
ushiroji 0:4c10d798d1bb 6 TB6612 motor_a(D10,D6,D7); //モータA制御用(pwma,ain1,ain2)
ushiroji 0:4c10d798d1bb 7 TB6612 motor_b(D11,D8,D9); //モータB制御用(pwmb,bin1,bin2)
ushiroji 0:4c10d798d1bb 8 Serial pc(USBTX,USBRX); //USBシリアル通信用
ushiroji 0:4c10d798d1bb 9 HMC6352 compass(D4, D5);
ushiroji 0:4c10d798d1bb 10 US015 hs(D2,D3);
ushiroji 0:4c10d798d1bb 11 int d;
ushiroji 0:4c10d798d1bb 12 int n;
ushiroji 0:4c10d798d1bb 13
ushiroji 0:4c10d798d1bb 14 int motor(char m) {
ushiroji 0:4c10d798d1bb 15 float motor_speed; //モータスピード情報格納用
ushiroji 0:4c10d798d1bb 16 while(1) {
ushiroji 0:4c10d798d1bb 17 motor_speed=0.5; //モータスピード(低速運転させるため２分の１の値とする。)
ushiroji 0:4c10d798d1bb 18 switch(m)
ushiroji 0:4c10d798d1bb 19 {
ushiroji 0:4c10d798d1bb 20 case '1': motor_a=motor_speed; //モータA正転
ushiroji 0:4c10d798d1bb 21 break;
ushiroji 0:4c10d798d1bb 22 case '2': motor_a=0; //モータAブレーキ
ushiroji 0:4c10d798d1bb 23 break;
ushiroji 0:4c10d798d1bb 24 case '3': motor_a=-motor_speed; //モータA逆転
ushiroji 0:4c10d798d1bb 25 break;
ushiroji 0:4c10d798d1bb 26 case '7': motor_b=motor_speed; //モータB正転
ushiroji 0:4c10d798d1bb 27 break;
ushiroji 0:4c10d798d1bb 28 case '8': motor_b=0; //モータBブレーキ
ushiroji 0:4c10d798d1bb 29 break;
ushiroji 0:4c10d798d1bb 30 case '9': motor_b=-motor_speed; //モータB逆転
ushiroji 0:4c10d798d1bb 31 break;
ushiroji 0:4c10d798d1bb 32 default : motor_a=0;
ushiroji 0:4c10d798d1bb 33 motor_b=0; //両方モータブレーキ
ushiroji 0:4c10d798d1bb 34 break;
ushiroji 0:4c10d798d1bb 35 }
ushiroji 0:4c10d798d1bb 36 }
ushiroji 0:4c10d798d1bb 37 }
ushiroji 0:4c10d798d1bb 38
ushiroji 0:4c10d798d1bb 39 int Compass()
ushiroji 0:4c10d798d1bb 40 {
ushiroji 0:4c10d798d1bb 41 int angle;
ushiroji 0:4c10d798d1bb 42 compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
ushiroji 0:4c10d798d1bb 43 angle = compass.sample() / 10;
ushiroji 0:4c10d798d1bb 44 return angle;
ushiroji 0:4c10d798d1bb 45 }
ushiroji 0:4c10d798d1bb 46
ushiroji 0:4c10d798d1bb 47 int Sonic()
ushiroji 0:4c10d798d1bb 48 {
ushiroji 0:4c10d798d1bb 49 int distance;
ushiroji 0:4c10d798d1bb 50 distance = hs.GetDistance();
ushiroji 0:4c10d798d1bb 51 return distance;
ushiroji 0:4c10d798d1bb 52 }
ushiroji 0:4c10d798d1bb 53
ushiroji 0:4c10d798d1bb 54 int main()
ushiroji 0:4c10d798d1bb 55 {
ushiroji 0:4c10d798d1bb 56 motor(1);
ushiroji 0:4c10d798d1bb 57 motor(7);
ushiroji 0:4c10d798d1bb 58 while(1)
ushiroji 0:4c10d798d1bb 59 {
ushiroji 0:4c10d798d1bb 60 n = Compass();
ushiroji 0:4c10d798d1bb 61 if(n >= 5) { //5°ずれると片方のモーターが逆転する
ushiroji 0:4c10d798d1bb 62 motor(3);
ushiroji 0:4c10d798d1bb 63 wait(5.0);
ushiroji 0:4c10d798d1bb 64 }
ushiroji 0:4c10d798d1bb 65 else {
ushiroji 0:4c10d798d1bb 66 motor(1);
ushiroji 0:4c10d798d1bb 67 motor(7);
ushiroji 0:4c10d798d1bb 68 }
ushiroji 0:4c10d798d1bb 69
ushiroji 0:4c10d798d1bb 70 d = Sonic();
ushiroji 0:4c10d798d1bb 71 if(d<=2000){ //超音波反応
ushiroji 0:4c10d798d1bb 72 motor(3);
ushiroji 0:4c10d798d1bb 73 wait(5.0);
ushiroji 0:4c10d798d1bb 74 motor(1);
ushiroji 0:4c10d798d1bb 75 }
ushiroji 0:4c10d798d1bb 76 }
ushiroji 0:4c10d798d1bb 77 }