File content as of revision 4:ca2e863a3d71:

#include "mbed.h"
#include "EC.h"
#include "SpeedController.h"
#include <math.h>

#define MEASURE_TIME 0.05
#define WHEEL_WIDTH 210
#define pi 3.141592

DigitalOut cylinder(p9);
AnalogIn LBR1(p17);
AnalogIn LBR2(p18);
AnalogIn LBR3(p19);
AnalogIn LBR4(p20);
PwmOut servo12(p21);
PwmOut servo3(p22);
PwmOut motor1(p25);
PwmOut motor2(p26);
DigitalOut motor1cw(p27);
DigitalOut motor2cw(p28);

Ec ec1(p11,p12,NC,500,MEASURE_TIME);//(PinName signalA , PinName signalB , PinName signalZ , int s=defsolution , double t=deftime)
Ec ec2(p14,p13,NC,500,MEASURE_TIME);
Timer t1;

int old3,old4,now3,now4 = 0;
double Kp = 1.0;//要調整
double Kd = 1000;//要調整
double Threshold1 = 0.5;//要調整
double Threshold2 = 0.5;//要調整
double Threshold3 = 0.5;//要調整
double Threshold4 = 0.5;//要調整
int s12open = 1000;//要修正
int s12close = 1000;//要修正
int s3up = 1000;//要修正
int s3down = 1000;//要修正
double turnDistance = WHEEL_WIDTH*pi*0.5;

void linetrace(double basePower);
void move(double RP,double LP,double RD,double LD);

int main()
{
    servo12.period_ms(20);
    servo3.period_ms(20);
    motor1.period_us(50);
    motor2.period_us(50);
    servo12.pulsewidth_us(s12close);
    servo3.pulsewidth_us(s3down);
    cylinder = 0;
    while(LBR1.read() > Threshold1 && LBR2.read() > Threshold2) {//ライントレース部分1
        linetrace(0.5);
    }
    servo12.pulsewidth_us(s12open);
    while(LBR1.read() < Threshold1 && LBR2.read() < Threshold2) {//ライントレース部分2
        linetrace(0.15);
    }
    t1.start();
    while(t1.read() < 2) { //ラインによる位置調整
        double kp = -1.0;//修正しろ
        motor1cw = signbit(LBR3.read() - Threshold3);//モーターのパワーの符号の逆
        motor2cw = signbit(LBR4.read() - Threshold4);//モーターのパワーの符号の逆
        motor1 = fabs(kp*(LBR3.read() - Threshold3));
        motor2 = fabs(kp*(LBR4.read() - Threshold4));
    }
    t1.reset();
    servo12.pulsewidth_us(s12close);//つかんで
    wait(1);
    servo3.pulsewidth_us(s3up);//持ち上げて
    wait(1);
    servo12.pulsewidth_us(s12open);//入れる
    wait(1);
    move(-0.6,-0.6,600,600);
    move(-0.6,0,turnDistance,0);
    move(0.6,0.6,600,600);
    move(0.6,0,turnDistance,0);
    move(0.6,0.6,300,300);
    cylinder = 1;//エアシリンダー
}


void linetrace(double basePower)
{
    old3 = now3;
    old4 = now4;
    now3 = 1000*LBR3.read();
    now4 = 1000*LBR4.read();
    double steering = Kp*(now3 - now4) + Kd*(now3 - now4 - (old3 - old4));
    motor1cw = !signbit(basePower + steering);//モーターのパワーの符号
    motor2cw = !signbit(basePower - steering);//モーターのパワーの符号
    motor1 = fabs(basePower + steering);
    motor2 = fabs(basePower - steering);
}



void move(double RP,double LP,double RD,double LD)//右モーターパワー(-1~1),左モーターパワー(-1~1),右モーター移動距離mm,左モーター移動距離mm
{
    ec1.setDiameter_mm(150);
    ec2.setDiameter_mm(150);
    ec1.reset();
    ec2.reset();
    while(fabs(ec1.getDistance_mm()) > RD && fabs(ec2.getDistance_mm()) > LD) {
        motor1cw = !signbit(RP); //モータ1の回転方向
        motor2cw = !signbit(LP); //モータ2の回転方向
        motor1 = fabs(RP); //モータ1への出力
        motor2 = fabs(LP); //モータ2への出力
    }
    motor1 = 0;
    motor2 = 0;
}