Diff: main.cpp

Revision:

0:c2319e0e250e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon May 28 05:16:46 2018 +0000
@@ -0,0 +1,160 @@
+#include"mbed.h"
+
+Serial device(p28,p27);//tx:left  rx:right
+Serial pc(USBTX, USBRX);//pcとの通信設定
+DigitalOut REDE(LED1);
+
+
+int data[2]={7,1};
+
+//通信設定
+void init(void){
+    device.baud(115200);
+    pc.baud(115200);
+    REDE = 0;
+}
+
+//ID書き換え
+void IDchange(unsigned char ID, unsigned char data){
+    
+    unsigned char Txdata[9]; //bytedata[9byte]
+    unsigned char CheckSum = 0;
+    
+    Txdata[0] = 0xFA; //Header
+    Txdata[1] = 0xAF; //Header
+    Txdata[2] = ID;   //ID
+    Txdata[3] = 0x00; //Flags
+    Txdata[4] = 0x04; //Adress
+    Txdata[5] = 0x01; //Length 
+    Txdata[6] = 0x01; //Count
+    Txdata[7] = data; //data
+    for(int i=2; i<=7; i++){
+         CheckSum = CheckSum ^ Txdata[i];
+    }
+    Txdata[8] = CheckSum;
+    
+    //Send Packet
+    REDE = 1;
+    for (int i=0; i<=8; i++){
+        device.putc(Txdata[i]);
+        pc.putc(Txdata[i]);
+    }
+    wait_us(250);
+    REDE = 0;
+}
+
+//データの書き込み
+void WriteROM(unsigned char ID){
+    
+    unsigned char Txdata[8]; //bytedata[8byte]
+    unsigned char CheckSum = 0;
+    
+    Txdata[0] = 0xFA; //Header
+    Txdata[1] = 0xAF; //Header
+    Txdata[2] = ID;   //ID
+    Txdata[3] = 0x40; //Flags
+    Txdata[4] = 0xFF; //Adress
+    Txdata[5] = 0x00; //Length 
+    Txdata[6] = 0x00; //Count
+    for(int i=2; i<=6; i++){
+         CheckSum = CheckSum ^ Txdata[i];
+    }
+    Txdata[7] = CheckSum;
+    
+    //Send Packet
+    REDE = 1;
+    for (int i=0; i<=7; i++){
+        device.putc(Txdata[i]);
+        pc.putc(Txdata[i]);
+    }
+    wait_us(250);
+    REDE = 0;
+}
+//ＰＣから割り込み時のデータ受け取り
+void getdata(){
+    for(int i=0;i<=1;i++){
+         data[i] = pc.getc();
+        }
+        
+     IDchange(data[0],data[1]);
+     WriteROM(data[1]);
+    }
+
+//トルク出力
+void torque(unsigned char ID, unsigned char data){
+    
+    unsigned char TxData[9]; //bytedata[9byte]
+    unsigned char CheckSum = 0; //CheckSum cakcukation
+    
+    TxData[0] = 0xFA; //Header
+    TxData[1] = 0xAF; //Header
+    TxData[2] = ID;   //ID
+    TxData[3] = 0x00; //Flags
+    TxData[4] = 0x24; //Adress
+    TxData[5] = 0x01; //Length 
+    TxData[6] = 0x01; //Count
+    TxData[7] = data; //data
+    
+    //CheckSum calculation
+    for(int i=2; i<=7; i++){
+         CheckSum = CheckSum ^ TxData[i];
+    }
+    TxData[8] = CheckSum;
+    
+    //Send Packet
+    REDE = 1;
+    for (int i=0; i<=8; i++){
+        device.putc(TxData[i]);
+     //   pc.putc(TxData[i]);
+    }
+    wait_us(250);
+    REDE = 0;
+}
+
+//ゴールポジション
+void GoalPosition (unsigned char ID, int data){
+    
+    unsigned char TxData[10];//10bytedata
+    unsigned char CheckSum = 0;
+    
+    TxData[0] = 0xFA; //Header
+    TxData[1] = 0xAF; //Header
+    TxData[2] = ID;   //ID
+    TxData[3] = 0x00; //Flags
+    TxData[4] = 0x1E; //Adress
+    TxData[5] = 0x02; //Length 
+    TxData[6] = 0x01; //Count
+    
+    TxData[7] = (unsigned char)0x00FF & data;
+    TxData[8] = (unsigned char)0x00FF & (data >> 8); 
+    
+    //CheckSum calculation
+    for(int i=2; i<=8; i++){
+         CheckSum = CheckSum ^ TxData[i];
+    }
+    TxData[9] = CheckSum;
+    
+    //Send Packet
+    REDE = 1;
+    for (int i=0; i<=9; i++){
+        device.putc(TxData[i]);
+        pc.putc(TxData[i]);
+    }
+    wait_us(250);
+    REDE = 0;
+}
+
+int main(){
+    init();
+   // pc.attach(getdata, Serial::RxIrq);
+    while(1){REDE = 1;
+     IDchange(data[0],data[1]);
+     WriteROM(data[1]);
+        torque(data[1],0x01);
+        wait(0.5);
+        GoalPosition(data[1],1500);
+        wait(2);
+        GoalPosition(data[1],-15000);
+        wait(2);
+    }
+}
\ No newline at end of file