2018F3RC3班 / Mbed 2 deprecated f3rc_3_auto_encode

f3rc 3班のエンコーダのプログラムです。

Dependencies:   EC2 Math mbed

main.cpp@2:a682612dd8b7, 2018-09-03 (annotated)

Committer:
nobu0103
Date:
Mon Sep 03 13:38:27 2018 +0000
Revision:
2:a682612dd8b7
Parent:
1:0ff59878537a
Child:
3:f3ef2e7a9407
f3rc 3? ???????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nobu0103 0:da183064372e 1 #include "mbed.h"
nobu0103 0:da183064372e 2 #include "Math.h"
nobu0103 2:a682612dd8b7 3 #include "EC.h"
nobu0103 0:da183064372e 4 #include "SpeedController.h"
nobu0103 0:da183064372e 5 DigitalIn cylinder(p9,PullUp);
nobu0103 0:da183064372e 6 DigitalIn cylindert(p10,PullUp);
nobu0103 0:da183064372e 7 DigitalIn encoder1a(p11);
nobu0103 0:da183064372e 8 DigitalIn encoder1b(p12);
nobu0103 0:da183064372e 9 DigitalIn encoder2a(p13);
nobu0103 0:da183064372e 10 DigitalIn encoder2b(p14);
nobu0103 0:da183064372e 11 AnalogIn LBR1(p17,PullUp);
nobu0103 0:da183064372e 12 AnalogIn LBR2(p18,PullUp);
nobu0103 0:da183064372e 13 AnalogIn LBR3(p19,PullUp);
nobu0103 0:da183064372e 14 AnalogIn LBR4(p20,PullUp);
nobu0103 0:da183064372e 15 PwmOut servo12(p21);
nobu0103 0:da183064372e 16 PwmOut servo3(p22);
nobu0103 0:da183064372e 17 PwmOut motor1(p25);
nobu0103 0:da183064372e 18 PwmOut motor2(p26);
nobu0103 0:da183064372e 19 DigitalIn motor1cw(p27,PullUp);
nobu0103 0:da183064372e 20 DigitalIn motor2cw(p28,PullUp);
nobu0103 0:da183064372e 21
nobu0103 0:da183064372e 22 EC wheel_1 (p11,p12,/*z相*/,/*分解能*/,0.05)
nobu0103 0:da183064372e 23 EC wheel_2 (p13,p14,/*z相*/,/*分解能*/,0.05)
nobu0103 0:da183064372e 24
nobu0103 1:0ff59878537a 25 /*直進用*/
nobu0103 2:a682612dd8b7 26 void straight(int l) //l=移動距離(mm)
nobu0103 0:da183064372e 27 {
nobu0103 0:da183064372e 28 mortor1.period_us(50);
nobu0103 0:da183064372e 29 mortor2.period_us(50);
nobu0103 2:a682612dd8b7 30 wheel_1.setDiameter_mm(/*測定輪半径(mm)*/);
nobu0103 2:a682612dd8b7 31 wheel_2.setDiameter_mm(/*測定輪半径(mm)*/);
nobu0103 2:a682612dd8b7 32 double EC1_distance = wheel_1.getDistance_mm (); //ec1の移動距離(mm)
nobu0103 2:a682612dd8b7 33 double EC2_distance = wheel_2.getDistance_mm (); //ec2の移動距離(mm)
nobu0103 2:a682612dd8b7 34
nobu0103 2:a682612dd8b7 35 while(EC1_distance <= l && EC2_distance <= l) {
nobu0103 1:0ff59878537a 36 motor1 = ;
nobu0103 1:0ff59878537a 37 motor2 = ;
nobu0103 1:0ff59878537a 38 motor1cw = ;
nobu0103 1:0ff59878537a 39 motor2cw = ;
nobu0103 1:0ff59878537a 40 }
nobu0103 1:0ff59878537a 41 }
nobu0103 1:0ff59878537a 42
nobu0103 1:0ff59878537a 43 /*後退用*/
nobu0103 2:a682612dd8b7 44 void straight(int l) //l=移動距離(mm)
nobu0103 1:0ff59878537a 45 {
nobu0103 1:0ff59878537a 46 mortor1.period_us(50);
nobu0103 1:0ff59878537a 47 mortor2.period_us(50);
nobu0103 2:a682612dd8b7 48 wheel_1.setDiameter_mm(/*測定輪半径(mm)*/);
nobu0103 2:a682612dd8b7 49 wheel_2.setDiameter_mm(/*測定輪半径(mm)*/);
nobu0103 2:a682612dd8b7 50 double EC1_distance = wheel_1.getDistance_mm (); //ec1の移動距離(mm)
nobu0103 2:a682612dd8b7 51 double EC2_distance = wheel_2.getDistance_mm (); //ec2の移動距離(mm)
nobu0103 2:a682612dd8b7 52
nobu0103 2:a682612dd8b7 53 while(EC1_distance <= l && EC2_distance <= l) {
nobu0103 0:da183064372e 54 motor1 = ;
nobu0103 0:da183064372e 55 motor2 = ;
nobu0103 0:da183064372e 56 motor1cw = ;
nobu0103 0:da183064372e 57 motor2cw = ;
nobu0103 0:da183064372e 58 }
nobu0103 0:da183064372e 59 }
nobu0103 0:da183064372e 60
nobu0103 0:da183064372e 61 /*右回転*/
nobu0103 2:a682612dd8b7 62 void straight(int l) //l=ecの移動距離(mm)
nobu0103 0:da183064372e 63 {
nobu0103 0:da183064372e 64 mortor1.period_us(50);
nobu0103 0:da183064372e 65 mortor2.period_us(50);
nobu0103 2:a682612dd8b7 66 wheel_1.setDiameter_mm(/*測定輪半径(mm)*/);
nobu0103 2:a682612dd8b7 67 wheel_2.setDiameter_mm(/*測定輪半径(mm)*/);
nobu0103 2:a682612dd8b7 68 double EC1_distance = wheel_1.getDistance_mm (); //ec1の移動距離(mm)
nobu0103 2:a682612dd8b7 69 double EC2_distance = wheel_2.getDistance_mm (); //ec2の移動距離(mm)
nobu0103 2:a682612dd8b7 70
nobu0103 2:a682612dd8b7 71 while(EC1_distance <= l && EC2_distance <= -1*l) {
nobu0103 0:da183064372e 72 motor1 = ;
nobu0103 0:da183064372e 73 motor2 = ;
nobu0103 0:da183064372e 74 motor1cw = ;
nobu0103 0:da183064372e 75 motor2cw = ;
nobu0103 0:da183064372e 76 }
nobu0103 0:da183064372e 77 }
nobu0103 0:da183064372e 78
nobu0103 0:da183064372e 79 /*左回転*/
nobu0103 2:a682612dd8b7 80 void straight(int l) //l=ecの移動距離(mm)
nobu0103 0:da183064372e 81 {
nobu0103 0:da183064372e 82 mortor1.period_us(50);
nobu0103 0:da183064372e 83 mortor2.period_us(50);
nobu0103 2:a682612dd8b7 84 wheel_1.setDiameter_mm(/*測定輪半径(mm)*/);
nobu0103 2:a682612dd8b7 85 wheel_2.setDiameter_mm(/*測定輪半径(mm)*/);
nobu0103 2:a682612dd8b7 86 double EC1_distance = wheel_1.getDistance_mm (); //ec1の移動距離(mm)
nobu0103 2:a682612dd8b7 87 double EC2_distance = wheel_2.getDistance_mm (); //ec2の移動距離(mm)
nobu0103 2:a682612dd8b7 88
nobu0103 2:a682612dd8b7 89 while(EC1_distance <= -1*l && EC2_distance <= l) {
nobu0103 0:da183064372e 90 motor1 = ;
nobu0103 0:da183064372e 91 motor2 = ;
nobu0103 0:da183064372e 92 motor1cw = ;
nobu0103 0:da183064372e 93 motor2cw = ;
nobu0103 0:da183064372e 94 }
nobu0103 0:da183064372e 95 }