main.cpp

00001 #include "mbed.h"
00002 #include "Math.h"
00003 #include "EC.h"
00004 #include "SpeedController.h"
00005 DigitalOut cylinder(p9);
00006 DigitalOut cylindert(p10);
00007 AnalogIn LBR1(p17,PullUp);
00008 AnalogIn LBR2(p18,PullUp);
00009 AnalogIn LBR3(p19,PullUp);
00010 AnalogIn LBR4(p20,PullUp);
00011 PwmOut servo12(p21);
00012 PwmOut servo3(p22);
00013 PwmOut motor1(p25);
00014 PwmOut motor2(p26);
00015 DigitalOut motor1cw(p27);
00016 DigitalOut motor2cw(p28);
00017 
00018 Ec ec1 (p12,p11,NC,500,0.05)
00019 Ec ec2 (p14,p13,NC,500,0.05)
00020 
00021 /*直進用*/
00022 void straight(int x)  //x=車輪の移動距離(mm)
00023 {
00024     mortor1.period_us(50);
00025     mortor2.period_us(50);
00026     ec1.setDiameter_mm(150);
00027     ec2.setDiameter_mm(150);
00028     ec1.reset();
00029     ec2.reset();
00030     double EC1_distance = ec1.getDistance_mm ();
00031     double EC2_distance = ec2.getDistance_mm ();
00032     while(fabs(EC1_distance) <= x && fabs(EC2_distance) <= x) {
00033         motor1 = ; //モータ1への出力
00034         motor2 = ; //モータ2への出力
00035         motor1cw = ; //モータ1の回転方向
00036         motor2cw = ; //モータ2の回転方向
00037     }
00038 }
00039 
00040 /*後退用*/
00041 void back(int x)  //x=車輪の移動距離(mm)
00042 {
00043     mortor1.period_us(50);
00044     mortor2.period_us(50);
00045     ec1.setDiameter_mm(150);
00046     ec2.setDiameter_mm(150);
00047     ec1.reset();
00048     ec2.reset();
00049     double EC1_distance = ec1.getDistance_mm ();
00050     double EC2_distance = ec2.getDistance_mm ();
00051     while(fabs(EC1_distance) <= x && fabs(EC2_distance) <= x) {
00052         motor1 = ; //モータ1への出力
00053         motor2 = ; //モータ2への出力
00054         motor1cw = ; //モータ1の回転方向
00055         motor2cw = ; //モータ2の回転方向
00056     }
00057 }
00058 
00059 /*右回転*/
00060 void right(int x)  //x=車輪の移動距離(mm)
00061 {
00062     mortor1.period_us(50);
00063     mortor2.period_us(50);
00064     ec1.setDiameter_mm(150);
00065     ec2.setDiameter_mm(150);
00066     ec1.reset();
00067     ec2.reset();
00068     double EC1_distance = ec1.getDistance_mm ();
00069     double EC2_distance = ec2.getDistance_mm ();
00070     while(fabs(EC1_distance) <= x && fabs(EC2_distance) <= x) {
00071         motor1 = ; //モータ1への出力
00072         motor2 = ; //モータ2への出力
00073         motor1cw = ; //モータ1の回転方向
00074         motor2cw = ; //モータ2の回転方向
00075     }
00076 }
00077 
00078 /*左回転*/
00079 void left(int x)  //x=車輪の移動距離(mm)
00080 {
00081     mortor1.period_us(50);
00082     mortor2.period_us(50);
00083     ec1.setDiameter_mm(150);
00084     ec2.setDiameter_mm(150);
00085     ec1.reset();
00086     ec2.reset();
00087     double EC1_distance = ec1.getDistance_mm ();
00088     double EC2_distance = ec2.getDistance_mm ();
00089     while(fabs(EC1_distance) <= x && fabs(EC2_distance) <= x) {
00090         motor1 = ; //モータ1への出力
00091         motor2 = ; //モータ2への出力
00092         motor1cw = ; //モータ1の回転方向
00093         motor2cw = ; //モータ2の回転方向
00094     }
00095 }
00096 
00097 int main()
00098 {
00099 }