File content as of revision 3:117706ca64d9:

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

#define MEASURE_TIME 0.05
#define WHEEL_WIDTH 202

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);
Timer t1;

int old3,old4,now3,now4 = 0;
double Kp = ;
double Kd = ;
double Threshold1 = ;
double Threshold2 = ;
double Threshold3 = ;
double Threshold4 = ;
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.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));
    }
    t1.reset();
    servo12.pulsewidth_us();//つかんで
    wait(1);
    servo3.pulsewidth_us();//持ち上げて
    wait(1);
    servo12.pulsewidth_us();//入れる
    wait(1);
    ec1.reset();
    sc2.reset();
    move()
    while(ec1.getDistance_mm() > 6778 || ec1.getDistance_mm() > 3245) {
        motor1cw = 0;
        motor2cw = 1;
        motor1 = 0.6;
        motor2 = 0.6;
    }
    while(ec1.getDistance_mm() > 3245) {
        motor2cw = 1;
        motor1 = 0;
        motor2 = 0.6;
    }
    while(ec1.getDistance_mm() > 6778 || ec1.getDistance_mm() > 3245) {
        motor1cw = 1;
        motor2cw = 0;
        motor1 = 0.6;
        motor2 = 0.6;
    }
    while(ec1.getDistance_mm() > 3245) {
        motor1cw = 1;
        motor1 = 0;
        motor2 = 0.6;
    }
    while(ec1.getDistance_mm() > 6778 || ec1.getDistance_mm() > 3245) {
        motor1cw = 1;
        motor2cw = 0;
        motor1 = 0.6;
        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);
}



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