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@2:d857188289be, 2018-09-04 (annotated)

Committer:
Shunsaku
Date:
Tue Sep 04 07:43:54 2018 +0000
Revision:
2:d857188289be
Parent:
1:0fe1b3af8053
Child:
3:117706ca64d9
2018/09/04

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Shunsaku 0:260f1d62f487 1 #include "mbed.h"
Shunsaku 2:d857188289be 2 #include "EC.h"
Shunsaku 1:0fe1b3af8053 3 #include "SpeedController.h"
Shunsaku 2:d857188289be 4 #include <math.h>
Shunsaku 2:d857188289be 5
Shunsaku 1:0fe1b3af8053 6 DigitalOut cylinder(p9);
Shunsaku 1:0fe1b3af8053 7 DigitalOut cylindert(p10);
Shunsaku 2:d857188289be 8 AnalogIn LBR1(p17);
Shunsaku 2:d857188289be 9 AnalogIn LBR2(p18);
Shunsaku 2:d857188289be 10 AnalogIn LBR3(p19);
Shunsaku 2:d857188289be 11 AnalogIn LBR4(p20);
Shunsaku 0:260f1d62f487 12 PwmOut servo12(p21);
Shunsaku 0:260f1d62f487 13 PwmOut servo3(p22);
Shunsaku 0:260f1d62f487 14 PwmOut motor1(p25);
Shunsaku 0:260f1d62f487 15 PwmOut motor2(p26);
Shunsaku 1:0fe1b3af8053 16 DigitalOut motor1cw(p27);
Shunsaku 1:0fe1b3af8053 17 DigitalOut motor2cw(p28);
Shunsaku 0:260f1d62f487 18
Shunsaku 2:d857188289be 19 Ec ec1(p11,p12,NC,500,MEASURE_TIME);//(PinName signalA , PinName signalB , PinName signalZ , int s=defsolution , double t=deftime)
Shunsaku 2:d857188289be 20 Ec ec2(p14,p13,NC,500,MEASURE_TIME);
Shunsaku 2:d857188289be 21 servo12.period_ms(20);
Shunsaku 2:d857188289be 22 servo3.period_ms(20);
Shunsaku 2:d857188289be 23 Timer t1;
Shunsaku 2:d857188289be 24
Shunsaku 1:0fe1b3af8053 25 int old3,old4,now3,now4 = 0;
Shunsaku 1:0fe1b3af8053 26 double Kp = ;
Shunsaku 1:0fe1b3af8053 27 double Kd = ;
Shunsaku 2:d857188289be 28 double Threshold1 = ;
Shunsaku 2:d857188289be 29 double Threshold2 = ;
Shunsaku 2:d857188289be 30 double Threshold3 = ;
Shunsaku 2:d857188289be 31 double Threshold4 = ;
Shunsaku 2:d857188289be 32 void linetrace(basePower);
Shunsaku 0:260f1d62f487 33
Shunsaku 0:260f1d62f487 34 int main()
Shunsaku 0:260f1d62f487 35 {
Shunsaku 1:0fe1b3af8053 36 motor1.period_us(50);
Shunsaku 1:0fe1b3af8053 37 motor2.period_us(50);
Shunsaku 2:d857188289be 38 servo12.period_ms(50);
Shunsaku 2:d857188289be 39 servo3.period_ms(50);
Shunsaku 2:d857188289be 40 servo12.pulsewidth_us();
Shunsaku 2:d857188289be 41 servo3.pulsewidth_us();
Shunsaku 2:d857188289be 42 while(LBR1.read() > Threshold1 && LBR2.read() > Threshold2) {//ライントレース部分1
Shunsaku 2:d857188289be 43 linetrace(0.5);
Shunsaku 2:d857188289be 44 }
Shunsaku 2:d857188289be 45 while(LBR1.read() < Threshold1 && LBR2.read() < Threshold2) {//ライントレース部分2
Shunsaku 2:d857188289be 46 linetrace(0.15);
Shunsaku 1:0fe1b3af8053 47 }
Shunsaku 2:d857188289be 48 t1.start();
Shunsaku 2:d857188289be 49 while(t1.read() < 2) { //ラインによる位置調整
Shunsaku 2:d857188289be 50 double kp = -;
Shunsaku 2:d857188289be 51 motor1cw = signbit(LBR3.read() - Threshold3);//モーターのパワーの符号の逆
Shunsaku 2:d857188289be 52 motor2cw = signbit(LBR4.read() - Threshold4);//モーターのパワーの符号の逆
Shunsaku 2:d857188289be 53 motor1 = fabs(kp*(LBR3.read() - Threshold3));
Shunsaku 2:d857188289be 54 motor2 = fabs(kp*(LBR4.read() - Threshold4));
Shunsaku 2:d857188289be 55 }
Shunsaku 2:d857188289be 56 servo12.pulsewidth_us();//つかんで
Shunsaku 2:d857188289be 57 wait(1);
Shunsaku 2:d857188289be 58 servo3.pulsewidth_us();//持ち上げて
Shunsaku 2:d857188289be 59 wait(1);
Shunsaku 2:d857188289be 60 servo12.pulsewidth_us();//入れる
Shunsaku 2:d857188289be 61 wait(1);
Shunsaku 2:d857188289be 62 ec1.reset();
Shunsaku 2:d857188289be 63 sc2.reset();
Shunsaku 2:d857188289be 64 while(ec1.getCount() > 6778 || ec1.getCount() > 3245) {
Shunsaku 1:0fe1b3af8053 65 motor1cw = 0;
Shunsaku 1:0fe1b3af8053 66 motor2cw = 1;
Shunsaku 2:d857188289be 67 motor1 = 0.6;
Shunsaku 2:d857188289be 68 motor2 = 0.6;
Shunsaku 2:d857188289be 69 }
Shunsaku 2:d857188289be 70 while(ec1.getCount() > 6778 || ec1.getCount() > 3245) {
Shunsaku 2:d857188289be 71 motor2cw = 1;
Shunsaku 2:d857188289be 72 motor1 = 0;
Shunsaku 2:d857188289be 73 motor2 = 0.6;
Shunsaku 1:0fe1b3af8053 74 }
Shunsaku 1:0fe1b3af8053 75 }
Shunsaku 1:0fe1b3af8053 76
Shunsaku 2:d857188289be 77
Shunsaku 2:d857188289be 78 void linetrace(double basePower)
Shunsaku 1:0fe1b3af8053 79 {
Shunsaku 1:0fe1b3af8053 80 old3 = now3;
Shunsaku 1:0fe1b3af8053 81 old4 = now4;
Shunsaku 1:0fe1b3af8053 82 now3 = 1000*LBR3.read();
Shunsaku 1:0fe1b3af8053 83 now4 = 1000*LBR4.read();
Shunsaku 1:0fe1b3af8053 84 double steering = Kp*(now3 - now4) + Kd*(now3 - now4 - (old3 - old4));
Shunsaku 2:d857188289be 85 motor1cw = !signbit(basePower + steering);//モーターのパワーの符号
Shunsaku 2:d857188289be 86 motor2cw = !signbit(basePower - steering);//モーターのパワーの符号
Shunsaku 2:d857188289be 87 motor1 = fabs(basePower + steering);
Shunsaku 2:d857188289be 88 motor2 = fabs(basePower - steering);
Shunsaku 0:260f1d62f487 89 }