2018F3RC3班
/
f3rc_3_auto_encode
f3rc 3班のエンコーダのプログラムです。
main.cpp@1:0ff59878537a, 2018-09-03 (annotated)
- Committer:
- nobu0103
- Date:
- Mon Sep 03 05:21:06 2018 +0000
- Revision:
- 1:0ff59878537a
- Parent:
- 0:da183064372e
- Child:
- 2:a682612dd8b7
f3rc 3????????????????
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 | 0:da183064372e | 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 | //SpeedControl (p11,p12,/*z相*/,/*分解能*/,0.05,,) pwmでないのでscは使えないかも |
nobu0103 | 0:da183064372e | 25 | //SpeedControl (p13,p14,/*z相*/,/*分解能*/,0.05,,) pwmでないのでscは使えないかも |
nobu0103 | 0:da183064372e | 26 | |
nobu0103 | 1:0ff59878537a | 27 | /*直進用*/ |
nobu0103 | 0:da183064372e | 28 | void straight(double t,int l) //t=時間 l=移動距離 |
nobu0103 | 0:da183064372e | 29 | { |
nobu0103 | 0:da183064372e | 30 | mortor1.period_us(50); |
nobu0103 | 0:da183064372e | 31 | mortor2.period_us(50); |
nobu0103 | 1:0ff59878537a | 32 | double EC1_omega = wheel_1.getOmega(); //ec1の角速度 |
nobu0103 | 1:0ff59878537a | 33 | double EC2_omega = wheel_2.getOmega(); //ec2の角速度 |
nobu0103 | 1:0ff59878537a | 34 | double L_1 = EC1_omega*"ecの半径"*t; |
nobu0103 | 1:0ff59878537a | 35 | double L_2 = EC2_omega*"ecの半径"*t; |
nobu0103 | 1:0ff59878537a | 36 | while(L_1 <= l && L_2 <= l) { |
nobu0103 | 1:0ff59878537a | 37 | motor1 = ; |
nobu0103 | 1:0ff59878537a | 38 | motor2 = ; |
nobu0103 | 1:0ff59878537a | 39 | motor1cw = ; |
nobu0103 | 1:0ff59878537a | 40 | motor2cw = ; |
nobu0103 | 1:0ff59878537a | 41 | } |
nobu0103 | 1:0ff59878537a | 42 | } |
nobu0103 | 1:0ff59878537a | 43 | |
nobu0103 | 1:0ff59878537a | 44 | /*後退用*/ |
nobu0103 | 1:0ff59878537a | 45 | void back(double t,int l) //t=時間 l=移動距離 |
nobu0103 | 1:0ff59878537a | 46 | { |
nobu0103 | 1:0ff59878537a | 47 | mortor1.period_us(50); |
nobu0103 | 1:0ff59878537a | 48 | mortor2.period_us(50); |
nobu0103 | 0:da183064372e | 49 | double EC1_omega = wheel_1.getOmega(); |
nobu0103 | 0:da183064372e | 50 | double EC2_omega = wheel_2.getOmega(); |
nobu0103 | 0:da183064372e | 51 | double L_1 = EC1_omega*"ecの半径"*t; |
nobu0103 | 0:da183064372e | 52 | double L_2 = EC2_omega*"ecの半径"*t; |
nobu0103 | 0:da183064372e | 53 | while(L_1 <= l && L_2 <= 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 | 0:da183064372e | 62 | void right(double t,int l) //t=時間 l=機体の中心を回転中心としたときのエンコーダの移動距離 |
nobu0103 | 0:da183064372e | 63 | { |
nobu0103 | 0:da183064372e | 64 | mortor1.period_us(50); |
nobu0103 | 0:da183064372e | 65 | mortor2.period_us(50); |
nobu0103 | 0:da183064372e | 66 | double EC1_omega = wheel_1.getOmega(); |
nobu0103 | 0:da183064372e | 67 | double EC2_omega = wheel_2.getOmega(); |
nobu0103 | 0:da183064372e | 68 | double L_1 = EC1_omega*"ecの半径"*t; |
nobu0103 | 0:da183064372e | 69 | double L_2 = EC2_omega*"ecの半径"*t; |
nobu0103 | 0:da183064372e | 70 | while(L_1 <= l && L_2 <= -1*l) { |
nobu0103 | 0:da183064372e | 71 | motor1 = ; |
nobu0103 | 0:da183064372e | 72 | motor2 = ; |
nobu0103 | 0:da183064372e | 73 | motor1cw = ; |
nobu0103 | 0:da183064372e | 74 | motor2cw = ; |
nobu0103 | 0:da183064372e | 75 | } |
nobu0103 | 0:da183064372e | 76 | } |
nobu0103 | 0:da183064372e | 77 | |
nobu0103 | 0:da183064372e | 78 | /*左回転*/ |
nobu0103 | 0:da183064372e | 79 | void left(double t,int l) //t=時間 l=l=機体の中心を回転中心としたときのエンコーダの移動距離 |
nobu0103 | 0:da183064372e | 80 | { |
nobu0103 | 0:da183064372e | 81 | mortor1.period_us(50); |
nobu0103 | 0:da183064372e | 82 | mortor2.period_us(50); |
nobu0103 | 0:da183064372e | 83 | double EC1_omega = wheel_1.getOmega(); |
nobu0103 | 0:da183064372e | 84 | double EC2_omega = wheel_2.getOmega(); |
nobu0103 | 0:da183064372e | 85 | double L_1 = EC1_omega*"ecの半径"*t; |
nobu0103 | 0:da183064372e | 86 | double L_2 = EC2_omega*"ecの半径"*t; |
nobu0103 | 0:da183064372e | 87 | while(L_1 <= -1*l && L_2 <= l) { |
nobu0103 | 0:da183064372e | 88 | motor1 = ; |
nobu0103 | 0:da183064372e | 89 | motor2 = ; |
nobu0103 | 0:da183064372e | 90 | motor1cw = ; |
nobu0103 | 0:da183064372e | 91 | motor2cw = ; |
nobu0103 | 0:da183064372e | 92 | } |
nobu0103 | 0:da183064372e | 93 | } |