ほぼ完成

Dependencies:   EC2 Math mbed

Fork of f3rc_3_auto_lintrace_905 by

## main.cpp

Committer:
Shunsaku
Date:
2018-09-12
Revision:
4:ca2e863a3d71
Parent:
3:117706ca64d9

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