Revision 3:56b034c41dc5, committed 2016-09-02

Comitter:

choutin

Date:

Fri Sep 02 06:50:14 2016 +0000

Parent:

2:4b2594dd86be

Child:

4:1604d599d40f

Commit message:

a

Changed in this revision

--- a/locate.h	Thu Sep 01 09:19:34 2016 +0000
+++ b/locate.h	Fri Sep 02 06:50:14 2016 +0000
@@ -39,6 +39,7 @@
 short v = 0;        //ステップ速度
 float x = 0, y = 0;     //xy方向に進んだ距離(m換算なし)
 float theta = 0;    //機体角度、x軸正の向きを0とする
+float erase_theta = 0;
 //宣言終わり
 
 
@@ -64,28 +65,6 @@
     return pulse;
 }
 
-void  update ()
-//位置情報を更新する。r,lはエンコーダから
-{
-    count1=__HAL_TIM_GET_COUNTER(&timer1);
-    dir1 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer1);
-    count2=__HAL_TIM_GET_COUNTER(&timer2);
-    dir2 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2);   
-    
-    r = -convert_enc_count(count1, dir1);
-    l = convert_enc_count(count2, dir2);
-        
-    theta = (r - l) * ROUND;
-    v = (r - pr + l - pl);
-
-    x += v * cos(theta);
-    y += v * sin(theta);
-
-    pr = r;
-    pl = l;
-    //pc.printf("count1:%d%s  count2:%d%s\r\n", count1, dir1==0 ? "+":"-",count2, dir2==0 ? "+":"-");
-    pc.printf("right:%d     left:%d    ", r, l);
-}
 
 short coordinateX()
 //xをmm換算して整数値として返す
@@ -106,4 +85,34 @@
 }
 
 
+void  update ()
+//位置情報を更新する。r,lはエンコーダから
+{
+    count1=__HAL_TIM_GET_COUNTER(&timer1);
+    dir1 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer1);
+    count2=__HAL_TIM_GET_COUNTER(&timer2);
+    dir2 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2);   
+    
+    r = -convert_enc_count(count1, dir1);
+    l = convert_enc_count(count2, dir2);
+        
+    theta = (r - l) * ROUND - erase_theta;
+    v = (r - pr + l - pl);
+
+    x += v * cos(theta);
+    y += v * sin(theta);
+
+    pr = r;
+    pl = l;
+    //pc.printf("count1:%d%s  count2:%d%s\r\n", count1, dir1==0 ? "+":"-",count2, dir2==0 ? "+":"-");
+    pc.printf("right:%d     left:%d     x:%d    y:%d    t:%f\n\r", r, l, coordinateX(), coordinateY(), coordinateTheta());
+}
+
+void erase()
+{
+    x = 0;
+    y = 0;
+    erase_theta = theta;
+}
+
 #endif

--- a/main.cpp	Thu Sep 01 09:19:34 2016 +0000
+++ b/main.cpp	Fri Sep 02 06:50:14 2016 +0000
@@ -1,13 +1,162 @@
 #include "mbed.h"
 #include "locate.h"
+#include "math.h"
+PwmOut M1cw(PA_11);
+PwmOut M1ccw(PB_15);
+PwmOut M2ccw(PB_14);
+PwmOut M2cw(PB_13);
+
+const int allowlength=5;
+const float allowdegree=0.02;
+const int rightspeed=29*2;
+const int leftspeed=31*2;
+const int turnspeed=15*2;
+
+const float PIfact=2.89;
+
+void initmotor()
+{
+
+
+    M1cw.period_us(256);
+    M1ccw.period_us(256);
+    M2cw.period_us(256);
+    M2ccw.period_us(256);
+
+}
+
+void move(int left,int right)
+{
+
+    float rightduty,leftduty;
+
+    if(right>256) {
+        right=256;
+    }
+    if(left>256) {
+        left=256;
+    }
+    if(right<-256) {
+        right=-256;
+    }
+    if(left<-256) {
+        left=-256;
+    }
+
+    rightduty=right/256.0;
+    leftduty=left/256.0;
+    if(right>0) {
+        M1cw.write(1-rightduty);
+        M1ccw.write(1);
+    } else {
+        M1cw.write(1);
+        M1ccw.write(1+rightduty);
+    }
+
+    if(left>0) {
+        M2cw.write(1-leftduty);
+        M2ccw.write(1);
+    } else {
+        M2cw.write(1);
+        M2ccw.write(1+leftduty);
+    }
+}
+
+
+void movelength(int length)
+{
+    int px,py,pt;
+    update();
+    px=coordinateX();
+    py=coordinateY();
+    pt=coordinateTheta();
+
+    move(rightspeed,leftspeed);
+    while(1) {
+
+        update();
+        //pc.printf("dx:%d, dy:%d, l:%d x:%d y:%d t:%f\n\r",px-coordinateX(),py-coordinateY(),length,coordinateX(),coordinateY(), coordinateTheta());
+        if(((px-coordinateX())*(px-coordinateX())+(py-coordinateY())*(py-coordinateY()))>length*length) {
+            break;
+        }
+
+    }
+    move(0,0);
+}
+
+void turncw()
+{
+    float pt;
+    move((-1)*turnspeed,turnspeed);
+
+    update();
+    pt=coordinateTheta();
+
+    while(1) {
+        update();
+        if(pt-coordinateTheta()>PIfact/2) {
+            move(0,0);
+            break;
+        }
+    }
+}
+
+void turnccw()
+{
+    float pt;
+    move(turnspeed,(-1)*turnspeed);
+
+    update();
+    pt=coordinateTheta();
+
+    while(1) {
+        update();
+        if(pt-coordinateTheta()<(-1)*PIfact/2) {
+            move(0,0);
+            break;
+        }
+    }
+}
+/*
+int main(){
+    setup();
+    initmotor();
+
+    for(int i=0; i < 4; i++)
+    {
+        movelength(1200);
+        turnccw();
+    }
+}*/
+
+void pmovex(int length){
+    int px,py,dx,dy;
+    int k=1;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
+    update();
+    px=coordinateX();
+    py=coordinateY();
+    move(rightspeed,leftspeed);
+        
+    while(1){
+        update();
+        dx=coordinateX()-px;
+        dy=coordinateY()-py;
+
+        if(dy>7){dy=7;}
+        if(dy<-7){dy=-7;}
+        move(rightspeed-k*dy,leftspeed+k*dy);
+        
+        if(dx>length){
+            move(0,0);
+            break;
+        }
+    }
+}
 
 int main()
 {
     setup();
+    initmotor();
 
-    while(1) {
-        pc.printf("a");
-        update();
-        pc.printf("x:%d     y:%d    t:%f\r\n", coordinateX(), coordinateY(), coordinateTheta());
-    }
-}
+
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/prime.h	Fri Sep 02 06:50:14 2016 +0000
@@ -0,0 +1,301 @@
+
+/*
+primeではPINを制御する関数を扱う。
+
+以下一覧
+open,close hand
+open close arm
+
+step 
+move
+blueorred
+lrsensor
+
+全関数共通で意図しない動作が起きたら基板上のLEDを点灯
+*/
+PwmOut pwmarm(PC_6);
+PwmOut pwmhand(PC_8);
+
+AnalogIn rightsensor(A0);
+AnalogIn leftsensor(A1);
+
+AnalogIn armadj(A2);
+AnalogIn handadj(A3);
+
+DigitalIn teamSW(PC_11);
+DigitalOut teamledblue(PC_10);
+DigitalOut teamledred(PC_12);
+
+DigitalOut errorled(LED1);
+
+
+DigitalIn phase1(PB_8);
+DigitalIn phase2(PC_9);
+DigitalIn phase4(PB_9);
+DigitalIn phase8(PD_2);
+
+PwmOut M1cw(PA_11);
+PwmOut M1ccw(PB_15);
+PwmOut M2ccw(PB_14);
+PwmOut M2cw(PB_13);
+
+DigitalOut PINW(PA_3);
+DigitalOut PINX(PA_2);
+DigitalOut PINY(PA_10);
+DigitalOut PINZ(PB_3);
+
+
+
+
+DigitalOut encordervcc1(PA_6);
+DigitalOut encordervcc2(PA_14);
+
+void initencorder(void){
+    encordervcc1=1;
+    encordervcc2=1;
+    }
+
+
+
+void close_hand(void) {
+     int i,degree;
+
+    pwmhand.period_ms(20);        //20ms
+    
+        degree=175;
+
+        i=500+degree*1900/180;
+        pwmhand.pulsewidth_us(i); 
+        
+        
+}
+
+void close_arm(void) {
+    PwmOut mypwm(PB_3);
+     int i,degree;
+
+    mypwm.period_ms(20);        //20ms
+    
+        degree=160;
+
+        i=500+degree*1900/180;
+        pwmarm.pulsewidth_us(i); 
+        
+        
+}
+
+
+
+void open_hand(void) {
+    PwmOut mypwm(PWM_OUT);
+     int i,degree;
+
+    pwmhand.period_ms(20);        //20ms
+    
+        degree=90;
+
+        i=500+degree*1900/180;
+        mypwm.pulsewidth_us(i); 
+        
+        
+}
+
+
+
+void open_arm(void) {
+    PwmOut mypwm(PWM_OUT);
+     int i,degree;
+
+    mypwm.period_ms(20);        //20ms
+    
+        degree=10;
+
+        i=500+degree*1900/180;
+        pwmarm.pulsewidth_us(i); 
+        
+        
+}
+
+void step(int degree){
+    
+    
+
+    int puls_times=0;
+
+    if(degree>0){
+
+        puls_times=1+(int)(degree/(3.75));
+        
+        while(1){
+            PINW=1;
+            PINX=1;
+            PINY=0;
+            PINZ=0;
+            wait_ms(20);
+            puls_times--;
+            
+            if(puls_times<0){break;}
+            
+            PINW=0;
+            PINX=1;
+            PINY=1;
+            PINZ=0;
+            wait_ms(20);
+            puls_times--;
+            
+            if(puls_times<0){break;}
+            
+            PINW=0;
+            PINX=0;
+            PINY=1;
+            PINZ=1;
+            wait_ms(20);
+            puls_times--;
+            
+            if(puls_times<0){break;}
+            
+            PINW=1;
+            PINX=1;
+            PINY=0;
+            PINZ=0;
+            wait_ms(20);
+            puls_times--;
+            
+            if(puls_times<0){break;}
+        }
+        
+    }    
+
+    if(degree<0){
+
+        puls_times=(-1)*(1+(int)(degree/(3.75)));
+        
+        while(1){
+            PINW=1;
+            PINX=1;
+            PINY=0;
+            PINZ=0;
+            wait_ms(20);
+            puls_times--;
+            
+            if(puls_times<0){break;}
+            
+            PINW=1;
+            PINX=0;
+            PINY=0;
+            PINZ=1;
+            wait_ms(20);
+            puls_times--;
+            
+            if(puls_times<0){break;}
+            
+            PINW=0;
+            PINX=0;
+            PINY=1;
+            PINZ=1;
+            wait_ms(20);
+            puls_times--;
+            
+            if(puls_times<0){break;}
+            
+            PINW=0;
+            PINX=1;
+            PINY=1;
+            PINZ=0;
+            wait_ms(20);
+            puls_times--;
+            
+            if(puls_times<0){break;}
+        }
+        
+    }    
+
+}
+
+
+
+int sensor(void){
+    //センサー読
+    int right ,left,x,y;
+    
+    right=rightsensor.read();
+    left=leftsensor.read();
+    
+    if(right>0.5){x=1;}
+    else{right=0;}
+    
+    if(left>0.5){y=1;}
+    else{left=0;}
+    
+    return x+2*(y);
+            //(right,left)=(off,off),(on,off),(off,on),(on,on)
+             //                 0        1         2       3
+}
+
+
+
+void initmotor(){
+
+    
+    M1cw.period_us(256);
+    M1ccw.period_us(256);
+    M2cw.period_us(256);
+    M2ccw.period_us(256);
+    
+}
+
+void move(int right,int left){
+    
+    float rightduty,leftduty;
+    
+    if(right>256){right=256;}
+    if(left>256){left=256;}
+    if(right<-256){right=-256;}
+    if(left<-256){left=-256;}
+
+    rightduty=right/256.0;
+    leftduty=left/256.0;
+    if(right>0){
+        M1cw.write(1-rightduty);
+        M1ccw.write(1);
+    }else{
+        M1cw.write(1);
+        M1ccw.write(1+rightduty);
+    }
+    
+    if(left>0){
+        M2cw.write(1-leftduty);
+        M2ccw.write(1);
+    }else{
+        M2cw.write(1);
+        M2ccw.write(1+leftduty);
+    }
+}
+    
+    
+int phase(void){
+    
+    phase1.mode(PullUp);
+    phase2.mode(PullUp);
+    phase4.mode(PullUp);
+    phase8.mode(PullUp);
+
+    int r=0;
+    
+    r=phase1+2*phase2+4*phase4+8*phase8;
+    
+    return r;
+}
+
+
+void teamLED(){
+   teamSW.mode(PullUp);
+   if(teamSW){
+       teamledblue=1;
+       teamledred=0;
+   }else{
+       teamledblue=0;
+       teamledred=1;
+
+   }
+}
\ No newline at end of file