Diff: main.cpp

Revision:

0:111abd91b0cb

Child:

1:3ae63be5592b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Dec 14 10:49:59 2018 +0000
@@ -0,0 +1,384 @@
+#include "mbed.h"
+#include "EC.h"
+#include "KondoServo.h"
+#include "hcsr04.h"
+
+Serial pc(USBTX,USBRX);
+Timer timer;
+
+//パラメタ
+double dutylimit = 0.6; // duty出力の上限値
+int resolution = 1000; //エンコーダの分解能×逓倍
+int ecgear = 35; //エンコーダのギアの歯数
+double Kp = 0.342000; //P値
+double Ki = 0.013062; //I値
+double Kd = 0.003266; //D値
+double leg_wait_time = 0.3; //脚の伸縮にかかる時間
+double turn_wait_time = 1.5; //旋回にかかる時間
+double target_max = 690; //直動機構の目標座標の上限（オーバーシュートを考慮して短めに設定）
+double target_min = 20; //直動機構の目標座標の下限（オーバーシュートを考慮して短めに設定）
+double rail_len = 710;
+double turn_error=10;   //旋回の際のY脚のずれの許容誤差
+double servo_iniposi=105;  //サーボ初期角度
+
+double target_plot[][2]={      //(mode,target)     mode 0→standby 1→straight 2→turn 3→sand dune 4→tussock 5→wait_shagai 6→mountain~finish
+    {0,0},
+    {1,3450},
+    {2,-45},
+    {3,0},
+    {2,-45},
+    {1,1030},
+    {2,90},
+    {1,2390},
+    {2,90},
+    {5,0},
+    {6,0}
+};
+
+
+
+//定数
+#define Pi 3.14159265359 //円周率π
+
+//変数
+double linear_posi=0;       //直線移動での距離 １プロットごとにリセットする?
+int target_num=0;           //現在の目標プロット
+
+
+
+//ピン宣言
+PwmOut motor_f(p21); //モータ正転
+PwmOut motor_b(p22); //モータ逆転
+DigitalIn switch_max(p9); //直動機構の上限のリミットスイッチ
+DigitalIn switch_min(p8); //直動機構の下限のリミットスイッチ
+DigitalIn switch_x(p7); //X脚の接地を判定するスイッチ（今後実装予定）
+DigitalIn switch_y(p5); //Y脚の接地を判定するスイッチ（今後実装予定）
+DigitalIn switch_hand(p6); //ゲルゲ回収アームのスイッチ
+DigitalOut air_x(p18); //X脚の伸縮
+DigitalOut air_y(p17); //Y脚の伸縮
+DigitalOut air_x_sub(p14); //X脚の先端の伸縮（今後実装予定）
+DigitalOut air_y_sub(p16); //Y脚の先端の伸縮（今後実装予定）
+DigitalOut air_hand(p19); //ゲルゲ回収アームのハンド部開閉
+DigitalOut air_trig(p20); //ゲルゲ持ち上げ機構のロック解除
+Ec ec(p24,p23,NC,resolution,0.01); //直動機構のエンコーダ
+KondoServo servo(p28,p27,1,115200); //旋回用のシリアルサーボ
+HCSR04 sensor(p30, p29);
+
+
+
+void setup();
+void out(double);//モーターの出力
+void move(double,double); //PID   (移動先の座標(機体内)[mm],許容誤差の絶対値[mm])
+void up_x();
+void down_x();
+void up_y();
+void down_y();
+void move_x (double,double);//X脚を移動
+void move_y (double,double);//Y脚を移動
+void reset();    //脚位置の初期化
+void turn(double);   //旋回動作
+void straight(double);   //目的地まで直進
+void wait_MR1();
+
+
+int main() {
+    setup();
+    while(switch_x==1){
+    }
+    
+    reset();
+    
+    /*while(1){
+        move(680,10);
+        pc.printf("%f , ",1.0*ecgear*Pi*ec.getCount()/resolution);
+        wait(0.5);
+        move(20,10);
+        pc.printf("%f\r\n",1.0*ecgear*Pi*ec.getCount()/resolution);
+        wait(0.5);
+    }*/ 
+    
+    //up_x();
+    /*while(1){
+    servo.set_degree(0,105);
+    wait(1);
+    servo.set_degree(0,135);
+    wait(1);
+    servo.set_degree(0,75);
+    wait(1);
+    }*/
+    //down_x();
+    
+    //reset();
+    
+    /*while(1){
+    out(0.2);
+    }*/
+    
+    //move_x(360,10);
+    //turn(90);
+    //turn(-45);
+    
+    
+    
+    
+    while(1) {
+        switch((int)(target_plot[target_num][0])){
+            case 0:
+                wait_MR1();
+                break;
+                
+            case 1:
+                straight(target_plot[target_num][1]);
+                pc.printf("%f\r\n",linear_posi);
+                //while(1);
+                break;
+                
+            case 2:
+                turn(target_plot[target_num][1]);
+                break;
+                
+            case 3:
+                move_x(target_max,10);
+                move_y(target_min,10);
+                move_x(target_max,10);
+                move_y(target_min,10);
+                move_x(rail_len*0.5,10);
+                
+            default:
+                break;
+        }
+        target_num++;
+        linear_posi=0;
+    }
+}
+
+double get_dist(){
+    sensor.start(); 
+    wait_ms(50);           //sensor.start()で信号を送ったあと反射波が帰ってきてから距離データが更新されるため、少し待つ必要がある。
+                           //何ループも回す場合は不要。また、時間は適当だから調整が必要。
+    return sensor.get_dist_cm();
+}
+
+
+
+//初期設定
+void setup()
+{
+    motor_f.period_us(100);
+    motor_b.period_us(100);
+    switch_max.mode(PullUp);
+    switch_min.mode(PullUp);
+    switch_x.mode(PullUp);
+    switch_y.mode(PullUp);
+    switch_hand.mode(PullUp);
+    air_x=0;
+    air_y=0;
+    air_x_sub=0;
+    air_y_sub=0;
+    air_hand=0;
+    air_trig=0;
+    servo.setSpeed(0,127);
+    wait_us(1000);
+    servo.init();
+    //servo.set_degree(0,servo_iniposi);
+}
+//モーターの出力
+void out (double duty)
+{
+    if(duty > 0) { //入力duty比が正の場合
+      if(switch_max == 0){ //上限のリミットスイッチが押されていない場合
+        if( fabs( duty ) < dutylimit ) { //制限値内
+            motor_f = fabs(duty);
+            motor_b = 0;
+        } else { //制限値超
+            motor_f = dutylimit;
+            motor_b = 0;
+        }
+      } else { //上限のリミットスイッチが押されている場合
+        motor_f = 0;
+        motor_b = 0;
+        //pc.printf("%f\r\n",1.0*ecgear*Pi*ec.getCount()/resolution);
+      }
+
+    } else {//入力duty比が負の場合
+        if(switch_min == 0){ //下限のリミットスイッチが押されていない場合
+            if( fabs(duty) < dutylimit) { //制限値内
+                motor_f = 0;
+                motor_b = fabs(duty);
+            } else { //制限値超
+                motor_f = 0;
+                motor_b = dutylimit;
+            }
+        }else { //下限のリミットスイッチが押されている場合
+            motor_f = 0;
+            motor_b = 0;
+        }
+    }
+}
+
+//PID制御
+void move(double target,double error) //(移動先の座標(機体内)[mm],許容誤差の絶対値[mm])
+{
+    double pile=0,deviation,distance,distance_old=0;
+    int i=0;
+    timer.start();
+    while(1) {
+        distance = 1.0*ecgear*Pi*ec.getCount()/resolution;
+        deviation = target - distance;
+        pile += deviation;
+        double period = timer.read();
+        out(deviation * Kp - (distance - distance_old) / period * Kd + pile *  Ki * period);
+        timer.reset();
+        distance_old = distance;
+        if (fabs(deviation) < error) {
+            i++;
+            if (i==3000) {
+                out(0);
+                timer.stop();
+                //pc.printf("%f\r\n",1.0*ecgear*Pi*ec.getCount()/resolution);
+                break;
+            }
+        } else {
+            i=0;
+        }
+    }
+}
+
+//X脚を上げる
+void up_x()
+{
+    air_x =1;
+    wait(leg_wait_time);
+}
+
+//X脚を降ろす
+void down_x()
+{
+    air_x =0;
+    wait(leg_wait_time);
+}
+
+//Y脚を上げる
+void up_y()
+{
+    air_y =1;
+    wait(leg_wait_time);
+}
+
+//Y脚を降ろす
+void down_y()
+{
+    air_y =0;
+    wait(leg_wait_time);
+}
+//X脚を移動
+void move_x (double x,double error)
+{
+    //pc.printf("x  target=%f , posi=%f\r\n",x,linear_posi);
+    double pre_posi=1.0*ecgear*Pi*ec.getCount()/resolution;
+    up_x();
+    if(x>target_max)x=target_max;
+    else if(x<target_min)x=target_min;
+    move(x,error);
+    down_x();
+    linear_posi+=1.0*ecgear*Pi*ec.getCount()/resolution-pre_posi;
+}
+
+//Y脚を移動
+void move_y (double y,double error)
+{
+    //pc.printf("y  target=%f , posi=%f\r\n",y,linear_posi);
+    up_y();
+    if(y>target_max)y=target_max;
+    else if(y<target_min)y=target_min;
+    move(y,error);
+    down_y();
+}
+
+int k=0;
+//脚位置の初期化
+void reset()
+{
+    //servo.set_degree(0,servo_iniposi);
+    up_x();
+    while(switch_min == 0) {
+        out(-0.2);
+        k++;
+        if(k>5000){
+            servo.set_degree(0,servo_iniposi);
+            k=0;
+        }
+    }
+    //pc.printf("%d\r\n",ec.getCount());
+    ec.reset();
+    out(0);
+    down_x();
+    //pc.printf("%d\r\n",ec.getCount());
+}
+
+//旋回動作
+void turn(double degree)
+{
+    if(fabs(1.0*ecgear*Pi*ec.getCount()/resolution-rail_len*0.5)>turn_error){
+        move_y(rail_len*0.5,turn_error);
+    }
+    bool turn_finish=0;
+    while(turn_finish==0){
+        double degree_out=0;
+        if(fabs(degree)<=45){
+            degree_out=degree;
+            turn_finish=1;
+        }else if(degree>45){
+            degree_out=45;
+            degree-=45;
+        }else{
+            degree_out=-45;
+            degree+=45;
+        }
+        up_y();
+        servo.set_degree(0,servo_iniposi+degree_out);
+        wait(turn_wait_time);
+        down_y();
+        up_x();
+        servo.set_degree(0,servo_iniposi);
+        wait(turn_wait_time);
+        down_x();
+    }
+}
+
+
+
+void straight(double target){
+    while(1){
+        double xleg_posi=1.0*ecgear*Pi*ec.getCount()/resolution;    //機体内のX脚位置
+        double dist=target-linear_posi;
+        pc.printf("leg=%f,dist=%f,linear_posi=%f\r\n",xleg_posi,dist,linear_posi);
+        if(fabs(dist)<10){
+            break;    
+        }else if((dist>0 && dist<=(target_max-xleg_posi)) || (dist<0 && dist>=(target_min-xleg_posi))){
+            move_x(xleg_posi+dist,10);
+        }else if(dist<0){
+            pc.printf("distance < 0");
+            break;
+        }else if(dist<=(target_max-target_min-10)){
+            if(dist<=(rail_len*0.5-target_min-10)){
+                move_y(dist+xleg_posi-rail_len*0.5,10);
+            }else{
+                move_y(target_min,10);
+            }
+        }else{
+            if(xleg_posi<(rail_len*0.5))move_x(target_max,10);
+            else move_y(target_min,10);
+        }
+    }
+}
+
+void wait_MR1(){
+    /*while(switch_x==1){
+        
+    }*/
+    while(switch_hand==0);
+    air_hand=1;
+    wait(1);
+    
+}
\ No newline at end of file