File content as of revision 2:d857188289be:

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

DigitalOut cylinder(p9);
DigitalOut cylindert(p10);
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);
servo12.period_ms(20);
servo3.period_ms(20);
Timer t1;

int old3,old4,now3,now4 = 0;
double Kp = ;
double Kd = ;
double Threshold1 = ;
double Threshold2 = ;
double Threshold3 = ;
double Threshold4 = ;
void linetrace(basePower);

int main()
{
    motor1.period_us(50);
    motor2.period_us(50);
    servo12.period_ms(50);
    servo3.period_ms(50);
    servo12.pulsewidth_us();
    servo3.pulsewidth_us();
    while(LBR1.read() > Threshold1 && LBR2.read() > Threshold2) {//ライントレース部分1
        linetrace(0.5);
    }
    while(LBR1.read() < Threshold1 && LBR2.read() < Threshold2) {//ライントレース部分2
        linetrace(0.15);
    }
    t1.start();
    while(t1.read() < 2) { //ラインによる位置調整
        double kp = -;
        motor1cw = signbit(LBR3.read() - Threshold3);//モーターのパワーの符号の逆
        motor2cw = signbit(LBR4.read() - Threshold4);//モーターのパワーの符号の逆
        motor1 = fabs(kp*(LBR3.read() - Threshold3));
        motor2 = fabs(kp*(LBR4.read() - Threshold4));
    }
    servo12.pulsewidth_us();//つかんで
    wait(1);
    servo3.pulsewidth_us();//持ち上げて
    wait(1);
    servo12.pulsewidth_us();//入れる
    wait(1);
    ec1.reset();
    sc2.reset();
    while(ec1.getCount() > 6778 || ec1.getCount() > 3245) {
        motor1cw = 0;
        motor2cw = 1;
        motor1 = 0.6;
        motor2 = 0.6;
    }
    while(ec1.getCount() > 6778 || ec1.getCount() > 3245) {
        motor2cw = 1;
        motor1 = 0;
        motor2 = 0.6;
    }
}


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);
}