Diff: main.cpp

Revision:

1:fa3a640a55e0

Parent:

0:651a188c5dc3

Child:

2:33681dfc2aa5

--- a/main.cpp	Thu Aug 23 05:26:41 2018 +0000
+++ b/main.cpp	Mon Dec 23 15:36:06 2019 +0000
@@ -1,26 +1,160 @@
 #include "mbed.h"
-DigitalInOut name(p8);//EN_IN(HIGHでICS機器に送信するモード、LOWでICS機器から受信するモードにするピン)と繋いでいる
-Serial master(p9, p10);//シリアル通信を担当するピン
-//Serial pc(USBTX, USBRX);実験中のため気にしないでください
-//char ret1,ret2,ret3;実験中のため気にしないでください
+#include "math.h"
+
+
+DigitalInOut name(PC_7);//EN_IN(HIGHでICS機器に送信するモード、LOWでICS機器から受信するモードにするピン)と繋いでいる
+Serial master(PA_9, PA_10);//シリアル通信を担当するピン
+
+Serial pc(USBTX,USBRX);
+bool setpos(int deg,int id);
+void getID();
+
+//int getpos(int id);
+int txlen=3;
+int rxlen=3;
+int _id;
+
 int main() {
-    master.baud(115200);//クロックレートの設定
+    pc.printf("hello\r\n");
+    master.baud(1250000);//クロックレートの設定
     master.format(8, Serial::Even, 1);//通信方式の設定
     name.output();//p8を出力モードに
-    name = 1;//EN_INにHIGH（送信モード）にする
-    master.putc(0x85);//ID5のサーボ（詳細ICS3.5/3.6コマンドリファレンスを参照してください）
-    master.putc(0x1B);//角度3500を110110101100と表し、さらに11011と0101100に分け1バイトずつ送信している
-    master.putc(0x2C);//11011 = 0x1B。0101100 = 0x2C。
-    wait(1.0);//送信した瞬間また次のコマンドを送信するとひとつ前のコマンドが送信しきれいないので連続での動作はできません。
-    //name.readable()を使おうと思いましたがうまく動作しないので便宜的にwaitを使っています。
-    master.putc(0x85);//一つ上に同じ
-    master.putc(0x59);//同じ
-    master.putc(0x6C);//同じ
-    wait(1.0);//同じ
-    /*name = 0;実験中のため気にしないでください
-    ret1 = master.getc();実験中のため気にしないでください
-    ret2 = master.getc();実験中のため気にしないでください
-    ret3 = master.getc();実験中のため気にしないでください
-    name = 1;実験中のため気にしないでください
-    pc.printf("");実験中のため気にしないでください*/
-}
\ No newline at end of file
+    
+    int deg=0;
+    char degpart[3];
+    int id;
+
+    while(1){
+    pc.printf("degree\r\n");
+    for(int i=0;i<3;i++){
+        degpart[i]=pc.getc()-48;
+        printf("%d",degpart[i]);      
+        }
+        printf("\r\n");
+        deg=100*degpart[0]+10*degpart[1]+degpart[2];
+    //pc.scanf("%d\r\n",deg);
+    pc.printf("%d\r\n",deg);
+    pc.printf("id\r\n");
+    id=pc.getc()-48;
+    //id+=48;
+    pc.printf("%d\r\n",id);
+    setpos(deg,id);
+    //getID();
+    } 
+}
+
+bool setpos(int deg,int id){
+    name=1;
+    int txdata[txlen];
+    int rxdata[rxlen];
+    if(deg>135 || deg<-135){
+        pc.printf("failed\r\n");
+        return 0;
+        }
+    int pos=deg*29.633;
+    pos=pos+7500;
+    //pc.printf("%d\r\n",pos);
+    txdata[0]=0x80+id;
+    txdata[1]=((pos >> 7) & 0x007F);
+    txdata[2]=(pos & 0x007F); 
+    
+    for(int i=0;i<txlen;i++){
+        master.putc(txdata[i]);
+        }
+        wait(0.00002);
+        
+        name=0;
+       
+        
+        for(int i=0;i<rxlen;i++){
+            rxdata[i]=master.getc();
+            //wait(0.05);
+            //pc.printf("%d\r\n",rxdata[i]);
+            }
+        //wait(0.01);
+        //name=1;
+            
+        //if(i==txlen-1){
+            //name=0;
+            
+            
+            /*for(int i=0;i<rxlen;i++){
+           //pc.printf("h\r\n");
+          
+           //pc.printf("hello\r\n");
+           rxdata[i]=master.getc();
+           }*/
+        
+        //pc.printf("OK\r\n");
+        
+    //wait(0.05);
+    //pc.printf("hello\r\n");
+  
+    
+       
+           //pc.printf("hello\r\n");
+        int degnow;  
+       degnow=((rxdata[1] << 7) & 0x3F80) + (rxdata[2] & 0x007F);
+    degnow=(degnow-7500)/29.6;
+    //pc.printf("%d\r\n",degnow);
+    pc.printf("%d\r\n",rxdata[0]);
+    pc.printf("%d\r\n",rxdata[1]);
+    pc.printf("%d\r\n",rxdata[2]);
+    pc.printf("角度%d\r\n",degnow);
+    }
+
+/*int getpos(int id){
+    name=1;
+    unsigned char txdata[txlen-1];
+    unsigned char rxdata[rxlen];
+    int degnow;
+    
+    txdata[0]=0xA0+id;
+    txdata[1]=0x05;
+    
+    pc.printf("done\r\n");
+    for(int i=0;i<txlen-1;i++){
+        master.putc(txdata[i]);
+        }
+    name=0;
+    wait(0.5);
+   if(master.readable()){
+    pc.printf("OK\r\n");
+    }
+    for(int i=0;i<rxlen;i++){
+        if(master.readable()){
+        rxdata[i]=master.getc();
+        pc.printf("%c",rxdata[i]);
+        }else{
+            pc.printf("failed\r\n");
+            }
+        }
+    
+    
+    
+    pc.printf("%d",rxdata[0]);
+    degnow=((rxdata[2] << 7) & 0x3F80) + (rxdata[3] & 0x007F);
+    //degnow=(degnow-7500)*1000/296;
+    pc.printf("%d",degnow);
+    return degnow;
+    } */
+
+void getID()
+    {
+        int id;
+        name=1;
+        master.putc(0xFF);
+        master.putc(0x0);
+        master.putc(0x0);
+        master.putc(0x0);
+        wait(0.01);
+        name=0;
+        /*for(int i=0;i<5;i++)
+        {
+            id = (master.getc() & 0x1f);
+        }*/
+        id=master.getc();
+        printf("%d\r\n",id);
+        wait_ms(500);
+        
+    }
\ No newline at end of file