ROBOSTEP_LIBRARY / Mbed 2 deprecated f3rc_3_auto_lintrace_912

ほぼ完成

Dependencies:   EC2 Math mbed

Fork of f3rc_3_auto_lintrace_905 by 2018F3RC3班

main.cpp@1:0fe1b3af8053, 2018-08-29 (annotated)

Committer:
Shunsaku
Date:
Wed Aug 29 06:31:52 2018 +0000
Revision:
1:0fe1b3af8053
Parent:
0:260f1d62f487
Child:
2:d857188289be
???????????EC2????????????cpp????????????????; ????????????Math????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Shunsaku 0:260f1d62f487 1 #include "mbed.h"
Shunsaku 1:0fe1b3af8053 2 #include "Math.h"
Shunsaku 1:0fe1b3af8053 3 #include "Ec.h"
Shunsaku 1:0fe1b3af8053 4 #include "SpeedController.h"
Shunsaku 1:0fe1b3af8053 5 DigitalOut cylinder(p9);
Shunsaku 1:0fe1b3af8053 6 DigitalOut cylindert(p10);
Shunsaku 0:260f1d62f487 7 DigitalIn encoder1a(p11);
Shunsaku 0:260f1d62f487 8 DigitalIn encoder1b(p12);
Shunsaku 0:260f1d62f487 9 DigitalIn encoder2a(p13);
Shunsaku 0:260f1d62f487 10 DigitalIn encoder2b(p14);
Shunsaku 0:260f1d62f487 11 AnalogIn LBR1(p17,PullUp);
Shunsaku 0:260f1d62f487 12 AnalogIn LBR2(p18,PullUp);
Shunsaku 0:260f1d62f487 13 AnalogIn LBR3(p19,PullUp);
Shunsaku 0:260f1d62f487 14 AnalogIn LBR4(p20,PullUp);
Shunsaku 0:260f1d62f487 15 PwmOut servo12(p21);
Shunsaku 0:260f1d62f487 16 PwmOut servo3(p22);
Shunsaku 0:260f1d62f487 17 PwmOut motor1(p25);
Shunsaku 0:260f1d62f487 18 PwmOut motor2(p26);
Shunsaku 1:0fe1b3af8053 19 DigitalOut motor1cw(p27);
Shunsaku 1:0fe1b3af8053 20 DigitalOut motor2cw(p28);
Shunsaku 0:260f1d62f487 21
Shunsaku 1:0fe1b3af8053 22 int old3,old4,now3,now4 = 0;
Shunsaku 1:0fe1b3af8053 23 double basePower;
Shunsaku 1:0fe1b3af8053 24 double Kp = ;
Shunsaku 1:0fe1b3af8053 25 double Kd = ;
Shunsaku 1:0fe1b3af8053 26 void linetrace;
Shunsaku 0:260f1d62f487 27
Shunsaku 0:260f1d62f487 28 int main()
Shunsaku 0:260f1d62f487 29 {
Shunsaku 1:0fe1b3af8053 30 motor1.period_us(50);
Shunsaku 1:0fe1b3af8053 31 motor2.period_us(50);
Shunsaku 1:0fe1b3af8053 32 motor1cw = 0;
Shunsaku 1:0fe1b3af8053 33 motor2cw = 1;
Shunsaku 1:0fe1b3af8053 34 basePower = 0.5;
Shunsaku 1:0fe1b3af8053 35 while(LBR1.read() > && LBR2.read() > ) {//ライントレース部分
Shunsaku 1:0fe1b3af8053 36 linetrace();
Shunsaku 1:0fe1b3af8053 37 }
Shunsaku 0:260f1d62f487 38
Shunsaku 1:0fe1b3af8053 39 while(/*条件をかけや*/) {//ラインによる位置調整
Shunsaku 1:0fe1b3af8053 40 double Kp = ;
Shunsaku 1:0fe1b3af8053 41 motor1cw = 0;
Shunsaku 1:0fe1b3af8053 42 motor2cw = 1;
Shunsaku 1:0fe1b3af8053 43 motor1 =
Shunsaku 1:0fe1b3af8053 44 motor2 =
Shunsaku 1:0fe1b3af8053 45 }
Shunsaku 1:0fe1b3af8053 46 }
Shunsaku 1:0fe1b3af8053 47
Shunsaku 1:0fe1b3af8053 48 void linetrace()
Shunsaku 1:0fe1b3af8053 49 {
Shunsaku 1:0fe1b3af8053 50 old3 = now3;
Shunsaku 1:0fe1b3af8053 51 old4 = now4;
Shunsaku 1:0fe1b3af8053 52 now3 = 1000*LBR3.read();
Shunsaku 1:0fe1b3af8053 53 now4 = 1000*LBR4.read();
Shunsaku 1:0fe1b3af8053 54 double steering = Kp*(now3 - now4) + Kd*(now3 - now4 - (old3 - old4));
Shunsaku 1:0fe1b3af8053 55 motor1 = basePower + steering;
Shunsaku 1:0fe1b3af8053 56 motor2 = basePower - steering;
Shunsaku 0:260f1d62f487 57 }