Revision 4:876bfa91934c, committed 2017-11-08

Comitter:

accelerator225

Date:

Wed Nov 08 06:24:17 2017 +0000

Parent:

3:61afd8d17063

Commit message:

1

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BufferedSerial.lib	Wed Nov 08 06:24:17 2017 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/sam_grove/code/BufferedSerial/#a0d37088b405

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/JY901.cpp	Wed Nov 08 06:24:17 2017 +0000
@@ -0,0 +1,93 @@
+
+#include "JY901.h"
+void JY901::receiveData()
+{
+    char ch;
+    while(mod.readable()){
+        ch = mod.getc();
+        parseInput(&ch, 1);
+    }
+}
+void JY901::parseCmpt(int token, unsigned char* payloadBuf, int payloadLen)
+{
+    switch(token){
+        case 0x51:
+            for (int i = 0; i < 3; ++i)
+            {
+                acc[i] = payloadBuf[i*2]|((int)payloadBuf[i*2+1]<<8);
+                acc[i] = acc[i] * 16 * 9.8 / 32768;
+            }
+            // lcd.cls();
+            // lcd.printf("Ax=%d\n",int(data[0]*100));
+            //pc.printf("Ax=%.2f\tAy=%.2f\tAz=%.2f\r\n", data[0], data[1], data[2]);
+            break;
+        case 0x52:
+            for (int i = 0; i < 3; ++i)
+            {
+                gyo[i] = payloadBuf[i*2]|((int)payloadBuf[i*2+1]<<8);
+                gyo[i] = gyo[i] * 2000 / 32768;
+            }
+            //pc.printf("Wx=%.2f\tWy=%.2f\tWz=%.2f\r\n", data[0], data[1], data[2]);
+            break;
+        case 0x53:
+            for (int i = 0; i < 3; ++i)
+            {
+                att[i] = payloadBuf[i*2]|((int)payloadBuf[i*2+1]<<8);
+                att[i] = att[i] * 180 / 32768;
+            }
+            // pc.printf("Roll=%.2f\tPitch=%.2f\tYaw=%.2f\r\n", data[0], data[1], data[2]);
+            break;
+        case 0x54:
+            for (int i = 0; i < 3; ++i)
+            {
+                mag[i] = payloadBuf[i*2]|((int)payloadBuf[i*2+1]<<8);
+            }
+            //pc.printf("Hx=%.2f\tHy=%.2f\tHz=%.2f\r\n", data[0], data[1], data[2]);
+            break;
+    }
+}
+void JY901::parseInput(const char* data, int len)
+{
+    for (int i = 0; i < len; ++i)
+    {
+        unsigned char ch = data[i], sum;
+        switch(state){
+            case 0:
+                if(ch == 0x55)
+                    state = 1;
+                break;
+            case 1:
+                token = ch;
+                if(0x51 <= token && token <= 0x54){
+                    payloadLen = 8;
+                    recvLen = 0;
+                    state = 2;
+                }else{
+                    // pc.printf("%s %x\r\n", "unknown token", token);
+                    state = 0;
+                }
+                break;
+            case 2:
+                payloadBuf[recvLen++] = ch;
+                if(recvLen == payloadLen){
+                    state = 3;
+                }
+                break;
+            case 3:
+                sum = 0x55;
+                sum += token;
+                for (int i = 0; i < payloadLen; ++i)
+                {
+                    sum += payloadBuf[i];
+                }
+                if(sum != ch){
+                    // pc.printf("wrong checksum\r\n");
+                }else{
+                    parseCmpt(token, payloadBuf, payloadLen);
+                    //myled = !myled;
+                }
+                state = 0;
+                break;
+        }
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/JY901.h	Wed Nov 08 06:24:17 2017 +0000
@@ -0,0 +1,36 @@
+#include "mbed.h"
+#include "BufferedSerial.h"
+class JY901
+{
+    int state, token, payloadLen, recvLen;
+    unsigned char payloadBuf[16];
+protected:
+    BufferedSerial mod;
+    float acc[3], gyo[3], mag[3], att[3];
+    void parseCmpt(int token, unsigned char* payloadBuf, int payloadLen);
+    void parseInput(const char* data, int len);
+public:
+    JY901(PinName TX, PinName RX) : mod(TX, RX, 32){}
+    ~JY901() {}
+    void receiveData();
+    void getAcc(float &x, float &y, float &z){
+        x = acc[0];
+        y = acc[1];
+        z = acc[2];
+    }
+    void getGyo(float &x, float &y, float &z){
+        x = gyo[0];
+        y = gyo[1];
+        z = gyo[2];
+    }
+    void getMag(float &x, float &y, float &z){
+        x = mag[0];
+        y = mag[1];
+        z = mag[2];
+    }
+    void getAttitude(float &roll, float &pitch, float &yaw){
+        roll = att[0];
+        pitch = att[1];
+        yaw = att[2];
+    }
+};
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SoftPWM.lib	Wed Nov 08 06:24:17 2017 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/komaida424/code/SoftPWM/#7918ce37626c

--- a/main.cpp	Wed Oct 11 06:29:40 2017 +0000
+++ b/main.cpp	Wed Nov 08 06:24:17 2017 +0000
@@ -1,78 +1,100 @@
 #include "mbed.h"
 #include "nRF24L01P.h"
-
-Serial pc(USBTX, USBRX, 115200); // tx, rx
+#include "SoftPWM.h"
+#define JY901transfersize 11
+#define TRANSFER_SIZE   11
+#include"JY901.h"
+#define pwm_period 1
+int flagg = 0;
 
-//                 mosi, miso, sck, csn, ce, irq
-nRF24L01P my_nrf24l01p(D4, D5, D3, D7, D8, D6);
-
-DigitalOut myled1(LED1);
-DigitalOut myled2(LED2);
+char targetdata[3];
+char receivedata[4];
+float change[4];
+int receivedatacnt = 0;
+SoftPWM PWM1(PA_2),PWM2(PA_3),PWM3(PA_6),PWM4(PA_7),PWM5(PB_0),PWM6(PB_1);
 
-int main() {
-    
-    pc.printf("init\r\n");
+int rollx=0,rolly=0,rollz=0,drx,dry,drz,ax,ay,az;
+float kp,ki,kd;
+void pwm_init()
+{
+    PWM1.period_ms(pwm_period);
+    PWM2.period_ms(pwm_period);
+    PWM3.period_ms(pwm_period);
+    PWM4.period_ms(pwm_period);
+    PWM5.period_ms(pwm_period);
+    PWM6.period_ms(pwm_period);
+    PWM1=0.0;PWM2=0.0;PWM3=0.0;PWM4=0.0;PWM5=0.0;PWM6=0.0;
+}
 
-// The nRF24L01+ supports transfers from 1 to 32 bytes, but Sparkfun's
-//  "Nordic Serial Interface Board" (http://www.sparkfun.com/products/9019)
-//  only handles 4 byte transfers in the ATMega code.
-#define TRANSFER_SIZE   4
-
-    char txData[TRANSFER_SIZE], rxData[TRANSFER_SIZE];
+JY901 _JY901(PA_9,PA_10);
+//nRF24L01P my_nrf24l01p(PB_15, PB_14, PB_13, PC_1, PC_2, PC_0);
+char txData[TRANSFER_SIZE], rxData[TRANSFER_SIZE];
+    int JY901cnt = 0;
     int txDataCnt = 0;
     int rxDataCnt = 0;
-
-    my_nrf24l01p.powerUp();
-
-    // Display the (default) setup of the nRF24L01+ chip
-    pc.printf( "nRF24L01+ Frequency    : %d MHz\r\n",  my_nrf24l01p.getRfFrequency() );
-    pc.printf( "nRF24L01+ Output power : %d dBm\r\n",  my_nrf24l01p.getRfOutputPower() );
-    pc.printf( "nRF24L01+ Data Rate    : %d kbps\r\n", my_nrf24l01p.getAirDataRate() );
-    pc.printf( "nRF24L01+ TX Address   : 0x%010llX\r\n", my_nrf24l01p.getTxAddress() );
-    pc.printf( "nRF24L01+ RX Address   : 0x%010llX\r\n", my_nrf24l01p.getRxAddress() );
-
-    pc.printf( "Type keys to test transfers:\r\n  (transfers are grouped into %d characters)\r\n", TRANSFER_SIZE );
-
-    my_nrf24l01p.setTransferSize( TRANSFER_SIZE );
+void transferdata(){
+    _JY901.receiveData();
+    float acc[3];
+            _JY901.getAttitude(acc[0], acc[1], acc[2]);
+           PWM1=0.2;//0.13+0.13*(float)rollx/32767.0;
+        PWM2=0.4;//0.13-0.13*(float)rolly/32767.0;
+        PWM3=0.1-0.1*acc[0]/180;
+}
 
-    my_nrf24l01p.setReceiveMode();
-    my_nrf24l01p.enable();
-
-    while (1) {
-
-        // If we've received anything over the host serial link...
-        if ( pc.readable() ) {
-
-            // ...add it to the transmit buffer
-            txData[txDataCnt++] = pc.getc();
-
-            // If the transmit buffer is full
-            if ( txDataCnt >= sizeof( txData ) ) {
+int PIDcontrol(char *targetdata,char *data,float *change){
+    return 1;
+}
 
-                // Send the transmitbuffer via the nRF24L01+
-                my_nrf24l01p.write( NRF24L01P_PIPE_P0, txData, txDataCnt );
-
-                txDataCnt = 0;
-            }
-
-            // Toggle LED1 (to help debug Host -> nRF24L01+ communication)
-            myled1 = !myled1;
-        }
-
-        // If we've received anything in the nRF24L01+...
-        if ( my_nrf24l01p.readable() ) {
+void update_roll()
+{
+   // char *s;
+    /*char data[JY901transfersize];
+    char flag =0,flag2 = 0;
+    int q=1;
+    while(q < 1000){
+        q++;
+        switch(flag){
+            case 0:
+                flag2=JY901.getc();
+                if(flag2==0x55) flag++;
+            break;
+            case 1:
+                flag2=JY901.getc();
+                if(flag2 == 0x53){
+                    flag++;
+                    data[0] = 0x55;
+                    data[1] = flag2;
+                    }
+                else flag--;
+            break;
+            case 2:
+                for(int i = 2;i != JY901transfersize;i++)
+                    data[i] = JY901.getc();
+                q = 1001;
+            break;
+        }        
+    }*/
 
-            // ...read the data into the receive buffer
-            rxDataCnt = my_nrf24l01p.read( NRF24L01P_PIPE_P0, rxData, sizeof( rxData ) );
-
-            // Display the receive buffer contents via the host serial link
-            for ( int i = 0; rxDataCnt > 0; rxDataCnt--, i++ ) {
+   // rollx+=drx;
+   // rolly+=dry;
+    //rollz+=drz;
+   // sprintf(s,"%f",rollx*180/32767);
+   // my_nrf24l01p.write( NRF24L01P_PIPE_P0,s,11);
+}
+int main() { 
+  //  my_nrf24l01p.powerUp();  
+   // my_nrf24l01p.setTransferSize( 11);
+   // my_nrf24l01p.setReceiveMode();
+  //  my_nrf24l01p.enable();  
 
-                pc.putc( rxData[i] );
-            }
+    Ticker time;
+    time.attach(&transferdata,0.1); 
+    while(1){
+   
+       // pc.printf("succes");
+ 
+    
+    } 
+       
+}
 
-            // Toggle LED2 (to help debug nRF24L01+ -> Host communication)
-            myled2 = !myled2;
-        }
-    }
-}

--- a/nRF24L01P.lib	Wed Oct 11 06:29:40 2017 +0000
+++ b/nRF24L01P.lib	Wed Nov 08 06:24:17 2017 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/Owen/code/nRF24L01P/#8ae48233b4e4
+https://os.mbed.com/users/accelerator225/code/send/#e0605b07372f