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@3:117706ca64d9, 2018-09-05 (annotated)

Committer:
Shunsaku
Date:
Wed Sep 05 07:40:36 2018 +0000
Revision:
3:117706ca64d9
Parent:
2:d857188289be
Child:
4:ca2e863a3d71
2018/09/05

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 3:117706ca64d9 6 #define MEASURE_TIME 0.05
Shunsaku 3:117706ca64d9 7 #define WHEEL_WIDTH 202
Shunsaku 3:117706ca64d9 8
Shunsaku 1:0fe1b3af8053 9 DigitalOut cylinder(p9);
Shunsaku 1:0fe1b3af8053 10 DigitalOut cylindert(p10);
Shunsaku 2:d857188289be 11 AnalogIn LBR1(p17);
Shunsaku 2:d857188289be 12 AnalogIn LBR2(p18);
Shunsaku 2:d857188289be 13 AnalogIn LBR3(p19);
Shunsaku 2:d857188289be 14 AnalogIn LBR4(p20);
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 2:d857188289be 22 Ec ec1(p11,p12,NC,500,MEASURE_TIME);//(PinName signalA , PinName signalB , PinName signalZ , int s=defsolution , double t=deftime)
Shunsaku 2:d857188289be 23 Ec ec2(p14,p13,NC,500,MEASURE_TIME);
Shunsaku 2:d857188289be 24 Timer t1;
Shunsaku 2:d857188289be 25
Shunsaku 1:0fe1b3af8053 26 int old3,old4,now3,now4 = 0;
Shunsaku 1:0fe1b3af8053 27 double Kp = ;
Shunsaku 1:0fe1b3af8053 28 double Kd = ;
Shunsaku 2:d857188289be 29 double Threshold1 = ;
Shunsaku 2:d857188289be 30 double Threshold2 = ;
Shunsaku 2:d857188289be 31 double Threshold3 = ;
Shunsaku 2:d857188289be 32 double Threshold4 = ;
Shunsaku 3:117706ca64d9 33 void linetrace(double basePower);
Shunsaku 3:117706ca64d9 34 void move(double RP,double LP,double RD,double LD);
Shunsaku 0:260f1d62f487 35
Shunsaku 0:260f1d62f487 36 int main()
Shunsaku 0:260f1d62f487 37 {
Shunsaku 3:117706ca64d9 38 servo12.period_ms(20);
Shunsaku 3:117706ca64d9 39 servo3.period_ms(20);
Shunsaku 1:0fe1b3af8053 40 motor1.period_us(50);
Shunsaku 1:0fe1b3af8053 41 motor2.period_us(50);
Shunsaku 2:d857188289be 42 servo12.period_ms(50);
Shunsaku 2:d857188289be 43 servo3.period_ms(50);
Shunsaku 2:d857188289be 44 servo12.pulsewidth_us();
Shunsaku 2:d857188289be 45 servo3.pulsewidth_us();
Shunsaku 2:d857188289be 46 while(LBR1.read() > Threshold1 && LBR2.read() > Threshold2) {//ライントレース部分1
Shunsaku 2:d857188289be 47 linetrace(0.5);
Shunsaku 2:d857188289be 48 }
Shunsaku 2:d857188289be 49 while(LBR1.read() < Threshold1 && LBR2.read() < Threshold2) {//ライントレース部分2
Shunsaku 2:d857188289be 50 linetrace(0.15);
Shunsaku 1:0fe1b3af8053 51 }
Shunsaku 2:d857188289be 52 t1.start();
Shunsaku 2:d857188289be 53 while(t1.read() < 2) { //ラインによる位置調整
Shunsaku 2:d857188289be 54 double kp = -;
Shunsaku 2:d857188289be 55 motor1cw = signbit(LBR3.read() - Threshold3);//モーターのパワーの符号の逆
Shunsaku 2:d857188289be 56 motor2cw = signbit(LBR4.read() - Threshold4);//モーターのパワーの符号の逆
Shunsaku 2:d857188289be 57 motor1 = fabs(kp*(LBR3.read() - Threshold3));
Shunsaku 2:d857188289be 58 motor2 = fabs(kp*(LBR4.read() - Threshold4));
Shunsaku 2:d857188289be 59 }
Shunsaku 3:117706ca64d9 60 t1.reset();
Shunsaku 2:d857188289be 61 servo12.pulsewidth_us();//つかんで
Shunsaku 2:d857188289be 62 wait(1);
Shunsaku 2:d857188289be 63 servo3.pulsewidth_us();//持ち上げて
Shunsaku 2:d857188289be 64 wait(1);
Shunsaku 2:d857188289be 65 servo12.pulsewidth_us();//入れる
Shunsaku 2:d857188289be 66 wait(1);
Shunsaku 2:d857188289be 67 ec1.reset();
Shunsaku 2:d857188289be 68 sc2.reset();
Shunsaku 3:117706ca64d9 69 move()
Shunsaku 3:117706ca64d9 70 while(ec1.getDistance_mm() > 6778 || ec1.getDistance_mm() > 3245) {
Shunsaku 1:0fe1b3af8053 71 motor1cw = 0;
Shunsaku 1:0fe1b3af8053 72 motor2cw = 1;
Shunsaku 2:d857188289be 73 motor1 = 0.6;
Shunsaku 2:d857188289be 74 motor2 = 0.6;
Shunsaku 2:d857188289be 75 }
Shunsaku 3:117706ca64d9 76 while(ec1.getDistance_mm() > 3245) {
Shunsaku 2:d857188289be 77 motor2cw = 1;
Shunsaku 2:d857188289be 78 motor1 = 0;
Shunsaku 2:d857188289be 79 motor2 = 0.6;
Shunsaku 1:0fe1b3af8053 80 }
Shunsaku 3:117706ca64d9 81 while(ec1.getDistance_mm() > 6778 || ec1.getDistance_mm() > 3245) {
Shunsaku 3:117706ca64d9 82 motor1cw = 1;
Shunsaku 3:117706ca64d9 83 motor2cw = 0;
Shunsaku 3:117706ca64d9 84 motor1 = 0.6;
Shunsaku 3:117706ca64d9 85 motor2 = 0.6;
Shunsaku 3:117706ca64d9 86 }
Shunsaku 3:117706ca64d9 87 while(ec1.getDistance_mm() > 3245) {
Shunsaku 3:117706ca64d9 88 motor1cw = 1;
Shunsaku 3:117706ca64d9 89 motor1 = 0;
Shunsaku 3:117706ca64d9 90 motor2 = 0.6;
Shunsaku 3:117706ca64d9 91 }
Shunsaku 3:117706ca64d9 92 while(ec1.getDistance_mm() > 6778 || ec1.getDistance_mm() > 3245) {
Shunsaku 3:117706ca64d9 93 motor1cw = 1;
Shunsaku 3:117706ca64d9 94 motor2cw = 0;
Shunsaku 3:117706ca64d9 95 motor1 = 0.6;
Shunsaku 3:117706ca64d9 96 motor2 = 0.6;
Shunsaku 3:117706ca64d9 97 }
Shunsaku 3:117706ca64d9 98 //エアシリンダー
Shunsaku 1:0fe1b3af8053 99 }
Shunsaku 1:0fe1b3af8053 100
Shunsaku 2:d857188289be 101
Shunsaku 2:d857188289be 102 void linetrace(double basePower)
Shunsaku 1:0fe1b3af8053 103 {
Shunsaku 1:0fe1b3af8053 104 old3 = now3;
Shunsaku 1:0fe1b3af8053 105 old4 = now4;
Shunsaku 1:0fe1b3af8053 106 now3 = 1000*LBR3.read();
Shunsaku 1:0fe1b3af8053 107 now4 = 1000*LBR4.read();
Shunsaku 1:0fe1b3af8053 108 double steering = Kp*(now3 - now4) + Kd*(now3 - now4 - (old3 - old4));
Shunsaku 2:d857188289be 109 motor1cw = !signbit(basePower + steering);//モーターのパワーの符号
Shunsaku 2:d857188289be 110 motor2cw = !signbit(basePower - steering);//モーターのパワーの符号
Shunsaku 2:d857188289be 111 motor1 = fabs(basePower + steering);
Shunsaku 2:d857188289be 112 motor2 = fabs(basePower - steering);
Shunsaku 3:117706ca64d9 113 }
Shunsaku 3:117706ca64d9 114
Shunsaku 3:117706ca64d9 115
Shunsaku 3:117706ca64d9 116
Shunsaku 3:117706ca64d9 117 void move(double RP,double LP,double RD,double LD)//右モーターパワー,左モーターパワー,右モーター移動距離mm,左モーター移動距離mm
Shunsaku 3:117706ca64d9 118 {
Shunsaku 3:117706ca64d9 119 mortor1.period_us(50);
Shunsaku 3:117706ca64d9 120 mortor2.period_us(50);
Shunsaku 3:117706ca64d9 121 ec1.setDiameter_mm(150);
Shunsaku 3:117706ca64d9 122 ec2.setDiameter_mm(150);
Shunsaku 3:117706ca64d9 123 ec1.reset();
Shunsaku 3:117706ca64d9 124 ec2.reset();
Shunsaku 3:117706ca64d9 125 while(fabs(ec1.getDistance_mm) <= RD && fabs(EC2_distance) <= LD) {
Shunsaku 3:117706ca64d9 126 motor1cw = signbit(RP); //モータ1の回転方向
Shunsaku 3:117706ca64d9 127 motor2cw = signbit(LP); //モータ2の回転方向
Shunsaku 3:117706ca64d9 128 motor1 = RP; //モータ1への出力
Shunsaku 3:117706ca64d9 129 motor2 = LP; //モータ2への出力
Shunsaku 3:117706ca64d9 130 }