2018F3RC3班
/
f3rc_3_auto_encode
f3rc 3班のエンコーダのプログラムです。
main.cpp@5:7d50cbf6674b, 2018-09-04 (annotated)
- Committer:
- nobu0103
- Date:
- Tue Sep 04 07:13:44 2018 +0000
- Revision:
- 5:7d50cbf6674b
- Parent:
- 4:dfb5f4860097
- Child:
- 6:e332ebb90091
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 | 4:dfb5f4860097 | 18 | EC ec1 (p12,p11,NC,500,0.05) |
nobu0103 | 4:dfb5f4860097 | 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 | 4:dfb5f4860097 | 28 | double EC1_distance = ec1.getDistance_mm (); |
nobu0103 | 4:dfb5f4860097 | 29 | double EC2_distance = ec2.getDistance_mm (); |
nobu0103 | 4:dfb5f4860097 | 30 | |
nobu0103 | 4:dfb5f4860097 | 31 | while(fabs(EC1_distance) <= x && fabs(EC2_distance) <= x) { |
nobu0103 | 5:7d50cbf6674b | 32 | motor1 = ; //モータ1への出力 |
nobu0103 | 5:7d50cbf6674b | 33 | motor2 = ; //モータ2への出力 |
nobu0103 | 5:7d50cbf6674b | 34 | motor1cw = ; //モータ1の回転方向 |
nobu0103 | 5:7d50cbf6674b | 35 | motor2cw = ; //モータ2の回転方向 |
nobu0103 | 1:0ff59878537a | 36 | } |
nobu0103 | 1:0ff59878537a | 37 | } |
nobu0103 | 1:0ff59878537a | 38 | |
nobu0103 | 1:0ff59878537a | 39 | /*後退用*/ |
nobu0103 | 4:dfb5f4860097 | 40 | void back(int x) //x=車輪の移動距離(mm) |
nobu0103 | 1:0ff59878537a | 41 | { |
nobu0103 | 1:0ff59878537a | 42 | mortor1.period_us(50); |
nobu0103 | 1:0ff59878537a | 43 | mortor2.period_us(50); |
nobu0103 | 4:dfb5f4860097 | 44 | ec1.setDiameter_mm(150); |
nobu0103 | 4:dfb5f4860097 | 45 | ec2.setDiameter_mm(150); |
nobu0103 | 4:dfb5f4860097 | 46 | double EC1_distance = ec1.getDistance_mm (); |
nobu0103 | 4:dfb5f4860097 | 47 | double EC2_distance = ec2.getDistance_mm (); |
nobu0103 | 4:dfb5f4860097 | 48 | |
nobu0103 | 5:7d50cbf6674b | 49 | while(fabs(EC1_distance) <= x && fabs(EC2_distance) <= x) { |
nobu0103 | 5:7d50cbf6674b | 50 | motor1 = ; //モータ1への出力 |
nobu0103 | 5:7d50cbf6674b | 51 | motor2 = ; //モータ2への出力 |
nobu0103 | 5:7d50cbf6674b | 52 | motor1cw = ; //モータ1の回転方向 |
nobu0103 | 5:7d50cbf6674b | 53 | motor2cw = ; //モータ2の回転方向 |
nobu0103 | 0:da183064372e | 54 | } |
nobu0103 | 0:da183064372e | 55 | } |
nobu0103 | 0:da183064372e | 56 | |
nobu0103 | 0:da183064372e | 57 | /*右回転*/ |
nobu0103 | 4:dfb5f4860097 | 58 | void right(int x) //x=車輪の移動距離(mm) |
nobu0103 | 0:da183064372e | 59 | { |
nobu0103 | 0:da183064372e | 60 | mortor1.period_us(50); |
nobu0103 | 0:da183064372e | 61 | mortor2.period_us(50); |
nobu0103 | 4:dfb5f4860097 | 62 | ec1.setDiameter_mm(150); |
nobu0103 | 4:dfb5f4860097 | 63 | ec2.setDiameter_mm(150); |
nobu0103 | 4:dfb5f4860097 | 64 | double EC1_distance = ec1.getDistance_mm (); |
nobu0103 | 4:dfb5f4860097 | 65 | double EC2_distance = ec2.getDistance_mm (); |
nobu0103 | 4:dfb5f4860097 | 66 | |
nobu0103 | 5:7d50cbf6674b | 67 | while(fabs(EC1_distance) <= x && fabs(EC2_distance) <= x) { |
nobu0103 | 5:7d50cbf6674b | 68 | motor1 = ; //モータ1への出力 |
nobu0103 | 5:7d50cbf6674b | 69 | motor2 = ; //モータ2への出力 |
nobu0103 | 5:7d50cbf6674b | 70 | motor1cw = ; //モータ1の回転方向 |
nobu0103 | 5:7d50cbf6674b | 71 | motor2cw = ; //モータ2の回転方向 |
nobu0103 | 0:da183064372e | 72 | } |
nobu0103 | 0:da183064372e | 73 | } |
nobu0103 | 0:da183064372e | 74 | |
nobu0103 | 0:da183064372e | 75 | /*左回転*/ |
nobu0103 | 4:dfb5f4860097 | 76 | void left(int x) //x=車輪の移動距離(mm) |
nobu0103 | 0:da183064372e | 77 | { |
nobu0103 | 0:da183064372e | 78 | mortor1.period_us(50); |
nobu0103 | 0:da183064372e | 79 | mortor2.period_us(50); |
nobu0103 | 4:dfb5f4860097 | 80 | ec1.setDiameter_mm(150); |
nobu0103 | 4:dfb5f4860097 | 81 | ec2.setDiameter_mm(150); |
nobu0103 | 4:dfb5f4860097 | 82 | double EC1_distance = ec1.getDistance_mm (); |
nobu0103 | 4:dfb5f4860097 | 83 | double EC2_distance = ec2.getDistance_mm (); |
nobu0103 | 4:dfb5f4860097 | 84 | |
nobu0103 | 5:7d50cbf6674b | 85 | while(fabs(EC1_distance) <= x && fabs(EC2_distance) <= x) { |
nobu0103 | 5:7d50cbf6674b | 86 | motor1 = ; //モータ1への出力 |
nobu0103 | 5:7d50cbf6674b | 87 | motor2 = ; //モータ2への出力 |
nobu0103 | 5:7d50cbf6674b | 88 | motor1cw = ; //モータ1の回転方向 |
nobu0103 | 5:7d50cbf6674b | 89 | motor2cw = ; //モータ2の回転方向 |
nobu0103 | 0:da183064372e | 90 | } |
nobu0103 | 3:f3ef2e7a9407 | 91 | } |
nobu0103 | 3:f3ef2e7a9407 | 92 | |
nobu0103 | 4:dfb5f4860097 | 93 | int main() |
nobu0103 | 4:dfb5f4860097 | 94 | { |
nobu0103 | 4:dfb5f4860097 | 95 | } |