Diff: main.cpp

Revision:

4:614e752487cc

Parent:

3:65eaf88bf278

--- a/main.cpp	Sun May 15 00:50:05 2016 +0000
+++ b/main.cpp	Fri Jun 24 11:38:21 2016 +0000
@@ -4,12 +4,12 @@
 #define LEAD_PACKET_MASK        0b10000000
 #define PACKET_ADDRESS_MASK     0b00011111
 #define PACKET_MODE_MASK        0b01100000
-#define PACKET_DATA_MASK        0b00111111
+#define PACKET_DATA_MASK        0b01111111
 
 //モータードライバの動作モード
-#define ORDER_FOLLOWING_MODE    0
-#define ENCODER_FOLLOWING_MODE  1
-#define SINGLE_ORDER_MODE       2 //単命令を受信した後は必ずsendback
+#define ORDER_FOLLOWING_MODE    1
+#define ENCODER_FOLLOWING_MODE  2
+#define SINGLE_ORDER_MODE       3 //単命令を受信した後は必ずsendback
 
 //単命令動作の種類
 #define S_ORDER_FREQUENCY           0
@@ -18,7 +18,7 @@
 #define S_ORDER_CERTAIN             3
 #define S_ORDER_SEND_ENCODER        4
 
-#define F_MD 18000 //PWMキャリア周波数設定
+#define F_MD 15000 //PWMキャリア周波数設定
 
 /*
  * High_A High_B
@@ -31,10 +31,10 @@
  */
  
 Serial comPort(dp16, dp15);
-DigitalOut driverEnable(dp7);
-DigitalOut LED1(LED1);
-DigitalOut LED2(LED2);
-BusIn switch5(dp4, dp6, dp18, dp25, dp26);
+DigitalOut driverEnable(dp17);
+DigitalOut LED(LED1);
+DigitalOut check_reciveData(LED2);
+BusIn switch5(dp26, dp25, dp18, dp6, dp4);
 //dp5 = ハイサイドPWM1, dp27 = ハイサイドPWM2 （オープンドレイン）
 //dp1 = ローサイドPWM1, dp2 = ローサイドPWM2
 PwmOut Low_A(dp1);
@@ -42,23 +42,44 @@
 DigitalOut High_A(dp5);
 DigitalOut High_B(dp27);
 
+Timer timer;
+
 AnalogIn ain(dp9);
 //PortIn encoder(Port0, 0b111);
 
 
 //グローバル変数
-uint8_t lastRXdata = 0;
+uint8_t lastRXdata[3] = {0};
 uint8_t address = 0;
 uint8_t mode = 0;
 uint8_t nowOrderAddress = 0;
 uint8_t dataNumber = 0;
 uint8_t sendPowerFlag = 0;
+uint8_t orderType = 0;
+uint8_t Ldir = 1;
+int FP_MD;
 
 void orderFollowManager(uint8_t reciveData, uint8_t dataNumber){
     switch (dataNumber) {
         case 2:
             break;
         case 3:
+            //comPort.printf("%d %d\n", lastRXdata[1], lastRXdata[2]);
+             if (lastRXdata[1]) {
+                High_A = 0;
+                Low_B.write(0);
+                if (Ldir == 0) wait_us(1000000 / F_MD);
+                High_B = lastRXdata[2] == 0 ? 0 : 1;
+                Low_A.write(1 / 127.0 * lastRXdata[2]);
+                Ldir = 1;
+            } else {
+                High_B = 0;
+                Low_A.write(0);
+                if (Ldir == 1) wait_us(1000000 / F_MD);
+                High_A = 1;
+                Low_B.write(1 - (1 / 127.0 * lastRXdata[2]));
+                Ldir = 0;
+            }
             break;
         case 4:
             break;
@@ -74,20 +95,28 @@
 }
 
 void com_rx() {
+    LED = !LED;
     static uint8_t reciveData;
     reciveData = comPort.getc();
+    //comPort.printf("%d  %f\n", reciveData, timer.read());
+    //timer.reset();
     
-    if (reciveData | LEAD_PACKET_MASK) {
-        nowOrderAddress = reciveData | PACKET_ADDRESS_MASK;
+    if (reciveData & LEAD_PACKET_MASK) {
+        nowOrderAddress = reciveData & PACKET_ADDRESS_MASK;
+        //comPort.printf("%d %d\n", nowOrderAddress, address);
         dataNumber = 0;
     }
     
+    //comPort.printf("%d\n", reciveData & PACKET_DATA_MASK);
+    
     if (nowOrderAddress == address) {
         dataNumber++;
-        lastRXdata = reciveData | PACKET_DATA_MASK;
+        lastRXdata[0] = lastRXdata[1];
+        lastRXdata[1] = lastRXdata[2];
+        lastRXdata[2] = reciveData & PACKET_DATA_MASK;
         
         if (dataNumber == 1) {
-            mode = (reciveData | PACKET_MODE_MASK) >> 5;
+            mode = (reciveData & PACKET_MODE_MASK) >> 5;
         } else {
             switch (mode) {
                 case ORDER_FOLLOWING_MODE:
@@ -105,11 +134,9 @@
 }
 
 int main() {
-    comPort.baud(9600);
-    comPort.format(8, Serial::Odd, 1);
-    comPort.attach(&com_rx, Serial::RxIrq);
+    //comPort.set_flow_control(Serial::RTS, dp17);
 
-    int FP_MD = 1000000 / F_MD; //PWM一周期の周期（マイクロ秒）
+    FP_MD = 1000000 / F_MD; //PWM一周期の周期（マイクロ秒）
     
     High_A = 0;
     High_B = 0;
@@ -119,16 +146,25 @@
     Low_B.write(0);
     
     switch5.mode(PullUp);
-    address = switch5.read();
+    address = ~switch5 & 0b00011111;
     comPort.printf("%d", address);
     //初期設定完了
     
-    LED = 1;
-    High_B = 1;
+    LED = 0;
+    check_reciveData = 0;
+    //High_B = 1;
     driverEnable = 1;
+    
+    timer.start();
+    
+    comPort.baud(115200);
+    comPort.format(8, Serial::None, 1);
+    comPort.attach(&com_rx, Serial::RxIrq);
     while (1) {
-        Low_A.write((ain.read() - 0.2) * 1.25 < 0 ? 0 : (ain.read() - 0.2) * 1.25);
+        //comPort.printf("%d\n", address);
+        //Low_A.write((ain.read() - 0.2) * 1.25 < 0 ? 0 : (ain.read() - 0.2) * 1.25);
         //pc.printf("%f\n", (ain.read() - 0.2) * 1.25 < 0 ? 0 : (ain.read() - 0.2) * 1.25);
-        comPort.printf("%d", 0b01010101);
+        //comPort.printf("%d\n", address);
+        //comPort.printf("%d\n", address);
     }
 }