main.cpp

00001 #include "mbed.h"
00002 #include "EC.h"
00003 #include "SpeedController.h"
00004 #include <math.h>
00005 
00006 #define MEASURE_TIME 0.05
00007 #define WHEEL_WIDTH 210
00008 #define pi 3.141592
00009 
00010 DigitalOut cylinder(p9);
00011 AnalogIn LBR1(p17);
00012 AnalogIn LBR2(p18);
00013 AnalogIn LBR3(p19);
00014 AnalogIn LBR4(p20);
00015 PwmOut servo12(p21);
00016 PwmOut servo3(p22);
00017 PwmOut motor1(p25);
00018 PwmOut motor2(p26);
00019 DigitalOut motor1cw(p27);
00020 DigitalOut motor2cw(p28);
00021 
00022 Ec ec1(p11,p12,NC,500,MEASURE_TIME);//(PinName signalA , PinName signalB , PinName signalZ , int s=defsolution , double t=deftime)
00023 Ec ec2(p14,p13,NC,500,MEASURE_TIME);
00024 Timer t1;
00025 
00026 int old3,old4,now3,now4 = 0;
00027 double Kp = 1.0;//要調整
00028 double Kd = 1000;//要調整
00029 double Threshold1 = 0.5;//要調整
00030 double Threshold2 = 0.5;//要調整
00031 double Threshold3 = 0.5;//要調整
00032 double Threshold4 = 0.5;//要調整
00033 int s12open = 1000;//要修正
00034 int s12close = 1000;//要修正
00035 int s3up = 1000;//要修正
00036 int s3down = 1000;//要修正
00037 double turnDistance = WHEEL_WIDTH*pi*0.5;
00038 
00039 void linetrace(double basePower);
00040 void move(double RP,double LP,double RD,double LD);
00041 
00042 int main()
00043 {
00044     servo12.period_ms(20);
00045     servo3.period_ms(20);
00046     motor1.period_us(50);
00047     motor2.period_us(50);
00048     servo12.pulsewidth_us(s12close);
00049     servo3.pulsewidth_us(s3down);
00050     cylinder = 0;
00051     while(LBR1.read() > Threshold1 && LBR2.read() > Threshold2) {//ライントレース部分1
00052         linetrace(0.5);
00053     }
00054     servo12.pulsewidth_us(s12open);
00055     while(LBR1.read() < Threshold1 && LBR2.read() < Threshold2) {//ライントレース部分2
00056         linetrace(0.15);
00057     }
00058     t1.start();
00059     while(t1.read() < 2) { //ラインによる位置調整
00060         double kp = -1.0;//修正しろ
00061         motor1cw = signbit(LBR3.read() - Threshold3);//モーターのパワーの符号の逆
00062         motor2cw = signbit(LBR4.read() - Threshold4);//モーターのパワーの符号の逆
00063         motor1 = fabs(kp*(LBR3.read() - Threshold3));
00064         motor2 = fabs(kp*(LBR4.read() - Threshold4));
00065     }
00066     t1.reset();
00067     servo12.pulsewidth_us(s12close);//つかんで
00068     wait(1);
00069     servo3.pulsewidth_us(s3up);//持ち上げて
00070     wait(1);
00071     servo12.pulsewidth_us(s12open);//入れる
00072     wait(1);
00073     move(-0.6,-0.6,600,600);
00074     move(-0.6,0,turnDistance,0);
00075     move(0.6,0.6,600,600);
00076     move(0.6,0,turnDistance,0);
00077     move(0.6,0.6,300,300);
00078     cylinder = 1;//エアシリンダー
00079 }
00080 
00081 
00082 void linetrace(double basePower)
00083 {
00084     old3 = now3;
00085     old4 = now4;
00086     now3 = 1000*LBR3.read();
00087     now4 = 1000*LBR4.read();
00088     double steering = Kp*(now3 - now4) + Kd*(now3 - now4 - (old3 - old4));
00089     motor1cw = !signbit(basePower + steering);//モーターのパワーの符号
00090     motor2cw = !signbit(basePower - steering);//モーターのパワーの符号
00091     motor1 = fabs(basePower + steering);
00092     motor2 = fabs(basePower - steering);
00093 }
00094 
00095 
00096 
00097 void move(double RP,double LP,double RD,double LD)//右モーターパワー(-1~1),左モーターパワー(-1~1),右モーター移動距離mm,左モーター移動距離mm
00098 {
00099     ec1.setDiameter_mm(150);
00100     ec2.setDiameter_mm(150);
00101     ec1.reset();
00102     ec2.reset();
00103     while(fabs(ec1.getDistance_mm()) > RD && fabs(ec2.getDistance_mm()) > LD) {
00104         motor1cw = !signbit(RP); //モータ1の回転方向
00105         motor2cw = !signbit(LP); //モータ2の回転方向
00106         motor1 = fabs(RP); //モータ1への出力
00107         motor2 = fabs(LP); //モータ2への出力
00108     }
00109     motor1 = 0;
00110     motor2 = 0;
00111 }