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@4:ca2e863a3d71, 2018-09-12 (annotated)

Committer:
Shunsaku
Date:
Wed Sep 12 02:37:14 2018 +0000
Revision:
4:ca2e863a3d71
Parent:
3:117706ca64d9 
9/12

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 4:ca2e863a3d71 7 #define WHEEL_WIDTH 210
Shunsaku 4:ca2e863a3d71 8 #define pi 3.141592
Shunsaku 3:117706ca64d9 9
Shunsaku 1:0fe1b3af8053 10 DigitalOut cylinder(p9);
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 4:ca2e863a3d71 27 double Kp = 1.0;//要調整
Shunsaku 4:ca2e863a3d71 28 double Kd = 1000;//要調整
Shunsaku 4:ca2e863a3d71 29 double Threshold1 = 0.5;//要調整
Shunsaku 4:ca2e863a3d71 30 double Threshold2 = 0.5;//要調整
Shunsaku 4:ca2e863a3d71 31 double Threshold3 = 0.5;//要調整
Shunsaku 4:ca2e863a3d71 32 double Threshold4 = 0.5;//要調整
Shunsaku 4:ca2e863a3d71 33 int s12open = 1000;//要修正
Shunsaku 4:ca2e863a3d71 34 int s12close = 1000;//要修正
Shunsaku 4:ca2e863a3d71 35 int s3up = 1000;//要修正
Shunsaku 4:ca2e863a3d71 36 int s3down = 1000;//要修正
Shunsaku 4:ca2e863a3d71 37 double turnDistance = WHEEL_WIDTH*pi*0.5;
Shunsaku 4:ca2e863a3d71 38
Shunsaku 3:117706ca64d9 39 void linetrace(double basePower);
Shunsaku 3:117706ca64d9 40 void move(double RP,double LP,double RD,double LD);
Shunsaku 0:260f1d62f487 41
Shunsaku 0:260f1d62f487 42 int main()
Shunsaku 0:260f1d62f487 43 {
Shunsaku 3:117706ca64d9 44 servo12.period_ms(20);
Shunsaku 3:117706ca64d9 45 servo3.period_ms(20);
Shunsaku 1:0fe1b3af8053 46 motor1.period_us(50);
Shunsaku 1:0fe1b3af8053 47 motor2.period_us(50);
Shunsaku 4:ca2e863a3d71 48 servo12.pulsewidth_us(s12close);
Shunsaku 4:ca2e863a3d71 49 servo3.pulsewidth_us(s3down);
Shunsaku 4:ca2e863a3d71 50 cylinder = 0;
Shunsaku 2:d857188289be 51 while(LBR1.read() > Threshold1 && LBR2.read() > Threshold2) {//ライントレース部分1
Shunsaku 2:d857188289be 52 linetrace(0.5);
Shunsaku 2:d857188289be 53 }
Shunsaku 4:ca2e863a3d71 54 servo12.pulsewidth_us(s12open);
Shunsaku 2:d857188289be 55 while(LBR1.read() < Threshold1 && LBR2.read() < Threshold2) {//ライントレース部分2
Shunsaku 2:d857188289be 56 linetrace(0.15);
Shunsaku 1:0fe1b3af8053 57 }
Shunsaku 2:d857188289be 58 t1.start();
Shunsaku 2:d857188289be 59 while(t1.read() < 2) { //ラインによる位置調整
Shunsaku 4:ca2e863a3d71 60 double kp = -1.0;//修正しろ
Shunsaku 2:d857188289be 61 motor1cw = signbit(LBR3.read() - Threshold3);//モーターのパワーの符号の逆
Shunsaku 2:d857188289be 62 motor2cw = signbit(LBR4.read() - Threshold4);//モーターのパワーの符号の逆
Shunsaku 2:d857188289be 63 motor1 = fabs(kp*(LBR3.read() - Threshold3));
Shunsaku 2:d857188289be 64 motor2 = fabs(kp*(LBR4.read() - Threshold4));
Shunsaku 2:d857188289be 65 }
Shunsaku 3:117706ca64d9 66 t1.reset();
Shunsaku 4:ca2e863a3d71 67 servo12.pulsewidth_us(s12close);//つかんで
Shunsaku 2:d857188289be 68 wait(1);
Shunsaku 4:ca2e863a3d71 69 servo3.pulsewidth_us(s3up);//持ち上げて
Shunsaku 2:d857188289be 70 wait(1);
Shunsaku 4:ca2e863a3d71 71 servo12.pulsewidth_us(s12open);//入れる
Shunsaku 2:d857188289be 72 wait(1);
Shunsaku 4:ca2e863a3d71 73 move(-0.6,-0.6,600,600);
Shunsaku 4:ca2e863a3d71 74 move(-0.6,0,turnDistance,0);
Shunsaku 4:ca2e863a3d71 75 move(0.6,0.6,600,600);
Shunsaku 4:ca2e863a3d71 76 move(0.6,0,turnDistance,0);
Shunsaku 4:ca2e863a3d71 77 move(0.6,0.6,300,300);
Shunsaku 4:ca2e863a3d71 78 cylinder = 1;//エアシリンダー
Shunsaku 1:0fe1b3af8053 79 }
Shunsaku 1:0fe1b3af8053 80
Shunsaku 2:d857188289be 81
Shunsaku 2:d857188289be 82 void linetrace(double basePower)
Shunsaku 1:0fe1b3af8053 83 {
Shunsaku 1:0fe1b3af8053 84 old3 = now3;
Shunsaku 1:0fe1b3af8053 85 old4 = now4;
Shunsaku 1:0fe1b3af8053 86 now3 = 1000*LBR3.read();
Shunsaku 1:0fe1b3af8053 87 now4 = 1000*LBR4.read();
Shunsaku 1:0fe1b3af8053 88 double steering = Kp*(now3 - now4) + Kd*(now3 - now4 - (old3 - old4));
Shunsaku 2:d857188289be 89 motor1cw = !signbit(basePower + steering);//モーターのパワーの符号
Shunsaku 2:d857188289be 90 motor2cw = !signbit(basePower - steering);//モーターのパワーの符号
Shunsaku 2:d857188289be 91 motor1 = fabs(basePower + steering);
Shunsaku 2:d857188289be 92 motor2 = fabs(basePower - steering);
Shunsaku 3:117706ca64d9 93 }
Shunsaku 3:117706ca64d9 94
Shunsaku 3:117706ca64d9 95
Shunsaku 3:117706ca64d9 96
Shunsaku 4:ca2e863a3d71 97 void move(double RP,double LP,double RD,double LD)//右モーターパワー(-1~1),左モーターパワー(-1~1),右モーター移動距離mm,左モーター移動距離mm
Shunsaku 3:117706ca64d9 98 {
Shunsaku 3:117706ca64d9 99 ec1.setDiameter_mm(150);
Shunsaku 3:117706ca64d9 100 ec2.setDiameter_mm(150);
Shunsaku 3:117706ca64d9 101 ec1.reset();
Shunsaku 3:117706ca64d9 102 ec2.reset();
Shunsaku 4:ca2e863a3d71 103 while(fabs(ec1.getDistance_mm()) > RD && fabs(ec2.getDistance_mm()) > LD) {
Shunsaku 4:ca2e863a3d71 104 motor1cw = !signbit(RP); //モータ1の回転方向
Shunsaku 4:ca2e863a3d71 105 motor2cw = !signbit(LP); //モータ2の回転方向
Shunsaku 4:ca2e863a3d71 106 motor1 = fabs(RP); //モータ1への出力
Shunsaku 4:ca2e863a3d71 107 motor2 = fabs(LP); //モータ2への出力
Shunsaku 4:ca2e863a3d71 108 }
Shunsaku 4:ca2e863a3d71 109 motor1 = 0;
Shunsaku 4:ca2e863a3d71 110 motor2 = 0;
Shunsaku 4:ca2e863a3d71 111 }