Diff: krs.cpp

Revision:

5:42c8babe5160

Parent:

4:b4c8d6671241

Child:

6:2f9e3f56b101

diff -r b4c8d6671241 -r 42c8babe5160 krs.cpp
--- a/krs.cpp	Sun Dec 29 16:36:37 2019 +0000
+++ b/krs.cpp	Sun Dec 13 05:29:00 2020 +0000
@@ -1,5 +1,6 @@
 #include "mbed.h"
 #include "krs.h"
+#include "param.h"
 
 krs::krs(PinName tx, PinName rx, PinName io, int baud) : master(tx,rx),en(io)
 {
@@ -8,19 +9,20 @@
         waittime=0.0002;
         }else if(baud==1250000){
             waittime=0.00002;
+            
             }
     master.format(8, Serial::Even, 1);
     en.output();
     }
 
-bool krs::setpos(int deg,int id){
+bool krs::setpos(float deg,int id){
     txlen=3;
     rxlen=3;
     int txdata[txlen];
     int rxdata[rxlen];
     en=1;
     
-    if(deg>135 || deg<-135){
+    if(deg>=135 || deg<=-135){
         printf("failed\r\n");
         return 0;
         }
@@ -37,23 +39,26 @@
         wait(waittime);
         //wait(0.00002);//1250000bps用
         //wait(0.0002);
-     /*   
+     #if 1 
        en=0;
         for(int i=0;i<rxlen;i++){
             rxdata[i]=master.getc();
             }
-    
+    #if 1
     int degnow;  
     degnow=((rxdata[1] << 7) & 0x3F80) + (rxdata[2] & 0x007F);
     degnow=(degnow-7500)/29.6;
    
-    /*pc.printf("%d\r\n",rxdata[0]);
-    pc.printf("%d\r\n",rxdata[1]);
-    pc.printf("%d\r\n",rxdata[2]);
-    printf("角度%d\r\n",degnow);*/
+   
+    printf("%d\r\n",rxdata[0]);
+    printf("%d\r\n",rxdata[1]);
+    printf("%d\r\n",rxdata[2]);
+    printf("角度%d\r\n",degnow);
+    #endif
+    #endif
     }
     
-void krs::getpos(int id){
+int krs::getpos(int id){
     en=1;
     txlen=2;
     rxlen=4;
@@ -76,11 +81,171 @@
         rxdata[i]=master.getc();
         }
 
-    printf("%d\r\n",rxdata[0]);
+    printf("motor ID: %d",rxdata[0]&0x1F);
     degnow=((rxdata[2] << 7) & 0x3F80) + (rxdata[3] & 0x007F);
     degnow=(degnow-7500)/29.6;
-    printf("%d\r\n",degnow);
-    
+    printf(" angle:%d\r\n",degnow);
+    return degnow;
     } 
 
- 
\ No newline at end of file
+void krs::getid(){
+    
+    txlen=4;
+    rxlen=1;
+    unsigned char txdata[txlen];
+    unsigned char rxdata[rxlen];
+    
+    en=1;
+    txdata[0]=0xFF;
+    txdata[1]=0x00;
+    txdata[2]=0x00;
+    txdata[3]=0x00;
+    
+    for(int i=0;i<txlen;i++){
+        master.putc(txdata[i]);
+        }
+    wait(waittime);
+    en=0;
+    
+    for(int i=0;i<rxlen;i++){
+        rxdata[i]=master.getc()&0x1F;
+        
+        }
+    printf("motor id is %d\r\n",rxdata[0]);
+    }
+
+void krs::setid(int id){
+    txlen=4;
+    rxlen=1;
+    unsigned char txdata[txlen];
+    unsigned char rxdata[rxlen];
+    
+    en=1;
+    txdata[0]=0xE0 + id;
+    txdata[1]=0x01;
+    txdata[2]=0x01;
+    txdata[3]=0x01;
+    
+    for(int i=0;i<txlen;i++){
+        master.putc(txdata[i]);
+        }
+    wait(waittime);
+    en=0;
+    
+    for(int i=0;i<rxlen;i++){
+        rxdata[i]=master.getc()&0x1F;
+        }
+    printf("motor id is %d\r\n",rxdata[0]);
+    
+    }
+
+void krs::setspeed(int id){
+    txlen=3;
+    rxlen=3;
+    unsigned char txdata[txlen];
+    unsigned char rxdata[rxlen];
+    
+    en=1;
+    txdata[0]=speed_cmd+id;
+    txdata[1]=speed_sc;
+    txdata[2]=max_speed;
+    
+    for(int i=0;i<txlen;i++){
+        rxdata[i]=master.putc(txdata[i]);
+        }
+    wait(waittime);
+    en=0;
+    
+    for(int i=0;i<rxlen;i++){
+        rxdata[i]=master.getc();
+        }
+    printf("motor %d speed is %d now\r\n",id,rxdata[2]);
+    }
+
+ void krs::setstrech(int id,int strech){
+    txlen=3;
+    rxlen=3;
+    unsigned char txdata[txlen];
+    unsigned char rxdata[rxlen];
+    
+    en=1;
+    txdata[0]=strech_cmd+id;
+    txdata[1]=strech_sc;
+    if(strech==1){
+    txdata[2]=weak_strech;
+    }else if(strech==2){
+        txdata[2]=mid_strech;
+        } else{
+                txdata[2]=max_strech;
+                }
+    master.putc(txdata[0]);
+    master.putc(txdata[1]);
+    master.putc(txdata[2]);
+
+    wait(waittime);
+    en=0;
+    
+    for(int i=0;i<rxlen;i++){
+        rxdata[i]=master.getc();
+        }
+    printf("motor %d strech is %d now\r\n",id,rxdata[2]);
+    }
+    
+void krs::set_slavemode_off(int id){
+    txlen=4;
+    rxlen=2;
+    
+    unsigned char txdata[txlen];
+    unsigned char rxdata[rxlen];
+    
+    en=1;
+    txdata[0]=efform_cmd+id;
+    txdata[1]=efform_sc;
+    txdata[2]=slave_off_up;
+    txdata[3]=slave_off_down;
+    
+    for(int i=0;i<txlen;i++){
+        master.putc(txdata[i]);
+        }
+    wait(waittime);
+    en=0;
+    
+    for(int i=0;i<rxlen;i++){
+        rxdata[i]=master.getc();
+        }
+    if(rxdata[0]==id && rxdata[1]==efform_sc){
+    printf("motor is not slave mode now\r\n");
+    }else {
+        printf("%d,%d\r\n",rxdata[0],rxdata[1]);
+        }
+    }
+    
+void krs::set_slavemode(int id){
+    txlen=4;
+    rxlen=2;
+    
+    unsigned char txdata[txlen];
+    unsigned char rxdata[rxlen];
+    
+    en=1;
+    txdata[0]=efform_cmd+id;
+    txdata[1]=efform_sc;
+    txdata[2]=slave_on_up;
+    txdata[3]=slave_on_down;
+    
+    for(int i=0;i<txlen;i++){
+        master.putc(txdata[i]);
+        }
+    
+    wait(waittime);
+    en=0;
+    
+    for(int i=0;i<rxlen;i++){
+        rxdata[i]=master.getc();
+        }
+    if(rxdata[0]==id && rxdata[1]==efform_sc){
+    printf("motor is slave mode now\r\n");
+    }else {
+        printf("%d,%d\r\n",rxdata[0],rxdata[1]);
+        }
+    }
\ No newline at end of file