F3RC1班
自動機 時間制限ver.
Diff: main.cpp
- Revision:
- 1:4b622e363fa1
- Parent:
- 0:2694d5a2f90a
--- a/main.cpp Fri Sep 01 23:50:06 2017 +0000
+++ b/main.cpp Sat Sep 02 06:17:48 2017 +0000
@@ -6,49 +6,50 @@
SPISlave spi(PB_15,PB_14,PB_13,PB_12);
Ec Ec1(PB_6,PC_7,NC,1048,0.05);
- Ticker ticker;
- Ticker ticker2;
- DigitalIn button(USER_BUTTON);
- Serial pc(USBTX,USBRX);
-
+Ticker ticker;
+Ticker ticker2;
+DigitalIn button(USER_BUTTON);
+Serial pc(USBTX,USBRX);
+
SpeedControl motor1(PA_10,PB_3,NC,1048,0.05,PA_1,PA_0); //left
SpeedControl motor2(PA_9,PA_8,NC,1048,0.05,PB_4,PB_5); //right
-int dis=0;
-int ang=0;
-
+int dis=0;
+int ang=0;
+
int n=1;
void calOmega() //角速度計算関数
{
- motor1.CalOmega();
- motor2.CalOmega();
- Ec1.CalOmega();
+ motor1.CalOmega();
+ motor2.CalOmega();
+ Ec1.CalOmega();
}
void st()
{
int kai=0;
- while(kai<50000){
- motor1.stop();
- motor2.stop();
- kai=kai + 1;
- }
+ while(kai<50000) {
+ motor1.stop();
+ motor2.stop();
+ kai=kai + 1;
+ }
}
-int Dis(){
- dis=0.301*Ec1.getCount();
+int Dis()
+{
+ dis=0.301*Ec1.getCount();
return dis;
- }
-
+}
+
/*void print()
{
// //////pc.printf("n=%d",n);
// //////pc.printf("count1=%d",Ec1.getCount());
- // //////pc.printf("gyro=%d",spi.read());
+ // //////pc.printf("gyro=%d",spi.read());
////pc.printf("count1=%f ",motor1.getOmega());
////pc.printf("count2=%f ",motor2.getOmega());
////pc.printf("duty1=%f ",motor1.duty);
@@ -57,201 +58,202 @@
void str(int a) //
{
-int kai=0;
-
- while(n==1){
- if(kai>=500){ //ループ500回ごとに角速度・出力duty比を表示
- // if(spi.receive()){
- ////pc.printf("count1=%f ",motor1.getOmega());
- ////pc.printf("count2=%f ",motor2.getOmega());
- ////pc.printf("duty1=%f ",motor1.duty);
- ////pc.printf("duty2=%f\r\n",motor2.duty);
- ////pc.printf("gyro=%d",spi.read());
+ int kai=0;
+
+ while(n==1) {
+ if(kai>=500) { //ループ500回ごとに角速度・出力duty比を表示
+ // if(spi.receive()){
+ ////pc.printf("count1=%f ",motor1.getOmega());
+ ////pc.printf("count2=%f ",motor2.getOmega());
+ ////pc.printf("duty1=%f ",motor1.duty);
+ ////pc.printf("duty2=%f\r\n",motor2.duty);
+ ////pc.printf("gyro=%d",spi.read());
kai=0;
- // }
- }
- if(Dis() < a){ //距離
- motor1.Sc(5);
- motor2.Sc(5);
- }else{
- n=2;
- }
- kai++;
- ////////////pc.printf("kai=%d\r\n",kai);
-
-}
-st();
+ // }
+ }
+ if(Dis() < a) { //距離
+ motor1.Sc(5);
+ motor2.Sc(5);
+ } else {
+ n=2;
+ }
+ kai++;
+ ////////////pc.printf("kai=%d\r\n",kai);
+
+ }
+ st();
}
void back(int a)
{
- int kai=0;
- while(n==2){
- kai++;
- if(kai>=500){ //ループ500回ごとに角速度・出力duty比を表示
- // if(spi.receive()){
- ////pc.printf("count1=%f ",motor1.getOmega());
- ////pc.printf("count2=%f ",motor2.getOmega());
- ////pc.printf("duty1=%f ",motor1.duty);
- ////pc.printf("duty2=%f\r\n",motor2.duty);
+ int kai=0;
+ while(n==2) {
+ kai++;
+ if(kai>=500) { //ループ500回ごとに角速度・出力duty比を表示
+ // if(spi.receive()){
+ ////pc.printf("count1=%f ",motor1.getOmega());
+ ////pc.printf("count2=%f ",motor2.getOmega());
+ ////pc.printf("duty1=%f ",motor1.duty);
+ ////pc.printf("duty2=%f\r\n",motor2.duty);
kai=0;
- // }
- }
- if(Dis() > a){
- motor1.Sc(-4.9);
- motor2.Sc(-5.2);
- }else{
- motor1.Sc(0);
- motor2.Sc(0);
- n=1;
- }
- }
- st();
+ // }
+ }
+
+ if(Dis() > a) {
+ motor1.Sc(-4.9);
+ motor2.Sc(-5.2);
+ } else {
+ motor1.Sc(0);
+ motor2.Sc(0);
+ n=1;
+ }
+ }
+ st();
}
void turnR(double b)
{
- // int kai=0;
- //int ang=0;
+// int kai=0;
+//int ang=0;
- if(b > 0){ //角
- motor1.Sc(5);
- motor2.Sc(0);
- wait(b);
-
- Ec1.reset();
- motor1.Sc(0);
- motor2.Sc(0);
- wait(0.5);
-
- }else{
- motor1.Sc(0);
- motor2.Sc(-5);
- wait(-b);
-
- Ec1.reset();
- motor1.Sc(0);
- motor2.Sc(0);
- wait(0.5);
-
-}
-n=1;
+ if(b > 0) { //角
+ motor1.Sc(5);
+ motor2.Sc(0);
+ wait(b);
+
+ Ec1.reset();
+ motor1.Sc(0);
+ motor2.Sc(0);
+ wait(0.5);
+
+ } else {
+ motor1.Sc(0);
+ motor2.Sc(-5);
+ wait(-b);
+
+ Ec1.reset();
+ motor1.Sc(0);
+ motor2.Sc(0);
+ wait(0.5);
+
+ }
+ n=1;
}
void turnL(double b)
{
- // int kai=0;
- //int ang=0;
+// int kai=0;
+//int ang=0;
- if(b > 0){ //角
- motor1.Sc(0);
- motor2.Sc(5);
- wait(0.815);
-
- Ec1.reset();
- motor1.Sc(0);
- motor2.Sc(0);
- wait(0.5);
-
-
- }else{
- motor1.Sc(-5);
- motor2.Sc(0);
- wait(0.81);
-
- Ec1.reset();
- motor1.Sc(0);
- motor2.Sc(0);
- wait(0.5);
-
-}
-n=1;
+ if(b > 0) { //角
+ motor1.Sc(0);
+ motor2.Sc(5);
+ wait(b);
+
+ Ec1.reset();
+ motor1.Sc(0);
+ motor2.Sc(0);
+ wait(0.5);
+
+
+ } else {
+ motor1.Sc(-5);
+ motor2.Sc(0);
+ wait(-b);
+
+ Ec1.reset();
+ motor1.Sc(0);
+ motor2.Sc(0);
+ wait(0.5);
+
+ }
+ n=1;
}
/*void turn(){
motor1.Sc(-5);
motor2.Sc(5);
wait(1.5);
-
-
+
+
}*/
-int main() {
+int main()
+{
servo.period_ms(20);
spi.format(16,3);
spi.frequency(1000000);
-
+
ticker.attach(&calOmega,0.05);
- // ticker2.attach(&print,0.5);
+ // ticker2.attach(&print,0.5);
motor1.setPDparam(0.3,0.4); //PDパラメータを設定
motor2.setPDparam(0.35,0.4); //PDパラメータを設定
-
+
motor1.setDOconstant(14.4); //duty比と角速度の変換係数を46.3に設定←設定必要かも
- motor2.setDOconstant(15.8);
-
-
+ motor2.setDOconstant(15.8);
+
+
sw.mode(PullUp);
-
+
+
+
+ if(sw==1) { //スタートゾーン１
-
- if(sw==1){ //スタートゾーン１
-
- servo.pulsewidth_us(900);
- str(395);
- turnL(0.8);
- str(1151);
- turnL(0.8);
- str(310);
- turnR(0.74);
- servo.pulsewidth_us(1800);
- str(1326);
- servo.pulsewidth_us(900);
- wait(0.5);
- back(0);
- turnR(-0.84);
- str(340);
- turnR(0.8);
- str(1131);
- turnR(0.8);
- str(375);
- servo.pulsewidth_us(1800);
- back(245);
- servo.pulsewidth_us(900);
- str(365);
-
-
- //押し出し
-
- }else{ //スタートゾーン２
- servo.pulsewidth_us(1800);
- str(400);
- servo.pulsewidth_us(900);
- wait(0.5); //
- turnL(0.8);
- str(1131);
- turnL(0.81);
- str(270);
- turnR(0.77);
- str(1131);
- turnR(0.77);
- str(380);
- servo.pulsewidth_us(1800);
- back(260);
- servo.pulsewidth_us(900);
- str(390);
-
-
-
-
-
- }
-
-
+ servo.pulsewidth_us(900);
+ str(395);
+ turnL(0.8);
+ str(1151);
+ turnL(0.8);
+ str(310);
+ turnR(0.74);
+ servo.pulsewidth_us(1800);
+ str(1326);
+ servo.pulsewidth_us(900);
+ wait(0.5);
+ back(0);
+ turnR(-0.84);
+ str(340);
+ turnR(0.8);
+ str(1131);
+ turnR(0.8);
+ str(375);
+ servo.pulsewidth_us(1800);
+ back(245);
+ servo.pulsewidth_us(900);
+ str(365);
+
+
+
+ } else { //スタートゾーン２
+ servo.pulsewidth_us(1800);
+ str(400);
+ servo.pulsewidth_us(900);
+ wait(0.5); //
+ turnL(0.8);
+ str(1131);
+ turnL(0.81);
+ str(270);
+ turnR(0.77);
+ str(1131);
+ turnR(0.77);
+ str(380);
+ servo.pulsewidth_us(1800);
+ back(260);
+ servo.pulsewidth_us(900);
+ str(390);
+
+
+
+
+
+ }
+
+
} //・距離とエンコーダ出力の関係 ・ジャイロの値