![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
f3rc 3班のエンコーダのプログラムです。
main.cpp@6:e332ebb90091, 2018-09-04 (annotated)
- Committer:
- nobu0103
- Date:
- Tue Sep 04 07:28:06 2018 +0000
- Revision:
- 6:e332ebb90091
- Parent:
- 5:7d50cbf6674b
f3rc ??????
Who changed what in which revision?
User | Revision | Line number | New 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 | 4:dfb5f4860097 | 5 | DigitalOut cylinder(p9); |
nobu0103 | 4:dfb5f4860097 | 6 | DigitalOut cylindert(p10); |
nobu0103 | 0:da183064372e | 7 | AnalogIn LBR1(p17,PullUp); |
nobu0103 | 0:da183064372e | 8 | AnalogIn LBR2(p18,PullUp); |
nobu0103 | 0:da183064372e | 9 | AnalogIn LBR3(p19,PullUp); |
nobu0103 | 0:da183064372e | 10 | AnalogIn LBR4(p20,PullUp); |
nobu0103 | 0:da183064372e | 11 | PwmOut servo12(p21); |
nobu0103 | 0:da183064372e | 12 | PwmOut servo3(p22); |
nobu0103 | 0:da183064372e | 13 | PwmOut motor1(p25); |
nobu0103 | 0:da183064372e | 14 | PwmOut motor2(p26); |
nobu0103 | 4:dfb5f4860097 | 15 | DigitalOut motor1cw(p27); |
nobu0103 | 4:dfb5f4860097 | 16 | DigitalOut motor2cw(p28); |
nobu0103 | 0:da183064372e | 17 | |
nobu0103 | 6:e332ebb90091 | 18 | Ec ec1 (p12,p11,NC,500,0.05) |
nobu0103 | 6:e332ebb90091 | 19 | Ec ec2 (p14,p13,NC,500,0.05) |
nobu0103 | 0:da183064372e | 20 | |
nobu0103 | 1:0ff59878537a | 21 | /*直進用*/ |
nobu0103 | 4:dfb5f4860097 | 22 | void straight(int x) //x=車輪の移動距離(mm) |
nobu0103 | 0:da183064372e | 23 | { |
nobu0103 | 0:da183064372e | 24 | mortor1.period_us(50); |
nobu0103 | 0:da183064372e | 25 | mortor2.period_us(50); |
nobu0103 | 4:dfb5f4860097 | 26 | ec1.setDiameter_mm(150); |
nobu0103 | 4:dfb5f4860097 | 27 | ec2.setDiameter_mm(150); |
nobu0103 | 6:e332ebb90091 | 28 | ec1.reset(); |
nobu0103 | 6:e332ebb90091 | 29 | ec2.reset(); |
nobu0103 | 4:dfb5f4860097 | 30 | double EC1_distance = ec1.getDistance_mm (); |
nobu0103 | 4:dfb5f4860097 | 31 | double EC2_distance = ec2.getDistance_mm (); |
nobu0103 | 4:dfb5f4860097 | 32 | while(fabs(EC1_distance) <= x && fabs(EC2_distance) <= x) { |
nobu0103 | 5:7d50cbf6674b | 33 | motor1 = ; //モータ1への出力 |
nobu0103 | 5:7d50cbf6674b | 34 | motor2 = ; //モータ2への出力 |
nobu0103 | 5:7d50cbf6674b | 35 | motor1cw = ; //モータ1の回転方向 |
nobu0103 | 5:7d50cbf6674b | 36 | motor2cw = ; //モータ2の回転方向 |
nobu0103 | 1:0ff59878537a | 37 | } |
nobu0103 | 1:0ff59878537a | 38 | } |
nobu0103 | 1:0ff59878537a | 39 | |
nobu0103 | 1:0ff59878537a | 40 | /*後退用*/ |
nobu0103 | 4:dfb5f4860097 | 41 | void back(int x) //x=車輪の移動距離(mm) |
nobu0103 | 1:0ff59878537a | 42 | { |
nobu0103 | 1:0ff59878537a | 43 | mortor1.period_us(50); |
nobu0103 | 1:0ff59878537a | 44 | mortor2.period_us(50); |
nobu0103 | 4:dfb5f4860097 | 45 | ec1.setDiameter_mm(150); |
nobu0103 | 4:dfb5f4860097 | 46 | ec2.setDiameter_mm(150); |
nobu0103 | 6:e332ebb90091 | 47 | ec1.reset(); |
nobu0103 | 6:e332ebb90091 | 48 | ec2.reset(); |
nobu0103 | 4:dfb5f4860097 | 49 | double EC1_distance = ec1.getDistance_mm (); |
nobu0103 | 4:dfb5f4860097 | 50 | double EC2_distance = ec2.getDistance_mm (); |
nobu0103 | 5:7d50cbf6674b | 51 | while(fabs(EC1_distance) <= x && fabs(EC2_distance) <= x) { |
nobu0103 | 5:7d50cbf6674b | 52 | motor1 = ; //モータ1への出力 |
nobu0103 | 5:7d50cbf6674b | 53 | motor2 = ; //モータ2への出力 |
nobu0103 | 5:7d50cbf6674b | 54 | motor1cw = ; //モータ1の回転方向 |
nobu0103 | 5:7d50cbf6674b | 55 | motor2cw = ; //モータ2の回転方向 |
nobu0103 | 0:da183064372e | 56 | } |
nobu0103 | 0:da183064372e | 57 | } |
nobu0103 | 0:da183064372e | 58 | |
nobu0103 | 0:da183064372e | 59 | /*右回転*/ |
nobu0103 | 4:dfb5f4860097 | 60 | void right(int x) //x=車輪の移動距離(mm) |
nobu0103 | 0:da183064372e | 61 | { |
nobu0103 | 0:da183064372e | 62 | mortor1.period_us(50); |
nobu0103 | 0:da183064372e | 63 | mortor2.period_us(50); |
nobu0103 | 4:dfb5f4860097 | 64 | ec1.setDiameter_mm(150); |
nobu0103 | 4:dfb5f4860097 | 65 | ec2.setDiameter_mm(150); |
nobu0103 | 6:e332ebb90091 | 66 | ec1.reset(); |
nobu0103 | 6:e332ebb90091 | 67 | ec2.reset(); |
nobu0103 | 4:dfb5f4860097 | 68 | double EC1_distance = ec1.getDistance_mm (); |
nobu0103 | 4:dfb5f4860097 | 69 | double EC2_distance = ec2.getDistance_mm (); |
nobu0103 | 5:7d50cbf6674b | 70 | while(fabs(EC1_distance) <= x && fabs(EC2_distance) <= x) { |
nobu0103 | 5:7d50cbf6674b | 71 | motor1 = ; //モータ1への出力 |
nobu0103 | 5:7d50cbf6674b | 72 | motor2 = ; //モータ2への出力 |
nobu0103 | 5:7d50cbf6674b | 73 | motor1cw = ; //モータ1の回転方向 |
nobu0103 | 5:7d50cbf6674b | 74 | motor2cw = ; //モータ2の回転方向 |
nobu0103 | 0:da183064372e | 75 | } |
nobu0103 | 0:da183064372e | 76 | } |
nobu0103 | 0:da183064372e | 77 | |
nobu0103 | 0:da183064372e | 78 | /*左回転*/ |
nobu0103 | 4:dfb5f4860097 | 79 | void left(int x) //x=車輪の移動距離(mm) |
nobu0103 | 0:da183064372e | 80 | { |
nobu0103 | 0:da183064372e | 81 | mortor1.period_us(50); |
nobu0103 | 0:da183064372e | 82 | mortor2.period_us(50); |
nobu0103 | 4:dfb5f4860097 | 83 | ec1.setDiameter_mm(150); |
nobu0103 | 4:dfb5f4860097 | 84 | ec2.setDiameter_mm(150); |
nobu0103 | 6:e332ebb90091 | 85 | ec1.reset(); |
nobu0103 | 6:e332ebb90091 | 86 | ec2.reset(); |
nobu0103 | 4:dfb5f4860097 | 87 | double EC1_distance = ec1.getDistance_mm (); |
nobu0103 | 4:dfb5f4860097 | 88 | double EC2_distance = ec2.getDistance_mm (); |
nobu0103 | 5:7d50cbf6674b | 89 | while(fabs(EC1_distance) <= x && fabs(EC2_distance) <= x) { |
nobu0103 | 5:7d50cbf6674b | 90 | motor1 = ; //モータ1への出力 |
nobu0103 | 5:7d50cbf6674b | 91 | motor2 = ; //モータ2への出力 |
nobu0103 | 5:7d50cbf6674b | 92 | motor1cw = ; //モータ1の回転方向 |
nobu0103 | 5:7d50cbf6674b | 93 | motor2cw = ; //モータ2の回転方向 |
nobu0103 | 0:da183064372e | 94 | } |
nobu0103 | 3:f3ef2e7a9407 | 95 | } |
nobu0103 | 3:f3ef2e7a9407 | 96 | |
nobu0103 | 4:dfb5f4860097 | 97 | int main() |
nobu0103 | 4:dfb5f4860097 | 98 | { |
nobu0103 | 4:dfb5f4860097 | 99 | } |