Diff: main.cpp

Revision:

0:69f8eade26c2

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jan 18 15:10:41 2017 +0000
@@ -0,0 +1,110 @@
+#include "mbed.h"
+
+// SANS LIBRAIRIE
+
+DigitalOut myled(LED1);
+I2C i2c(p9, p10);
+
+Serial pc(USBTX, USBRX); // tx, rx
+
+void initUART(unsigned int baudRate){
+    volatile int* const PCONP = (int*) 0x400FC0C4;
+    volatile int* const PCLKSEL1 = (int*) 0x400FC1AC;
+    volatile int* const U2LCR = (int*) 0x4009800C;
+    volatile int* const U2DLL = (int*) 0x40098000;
+    volatile int* const U2DLM = (int*) 0x40098004;
+    volatile int* const PINSEL0 = (int*) 0x4002C000;
+    
+    int const clock = 100000000;
+    int const clockDiv = 3;
+    int const peripheralClock = clock >> clockDiv;
+    int const dlmDll = peripheralClock / (baudRate << 4);
+    
+    *PCONP |= 0x1000000;
+    *PCLKSEL1 |= 0x30000;
+    *U2LCR |= 0x80;
+    *U2DLL = dlmDll & 0xFF;
+    *U2DLM = dlmDll & 0xFF00;
+    *U2LCR = 0x3;
+    *PINSEL0 |= 0x500000;
+}
+
+void sendCharToSS(char data){
+    volatile int* const U2LSR = (int*) 0x40098014;
+    volatile int* const THR = (int*) 0x40098000;
+    
+    while (*U2LSR & 0x20 == 0) wait_us(25);
+    
+    *THR = data;
+}
+
+int main() {
+    initUART(9600);
+    int addr = 0x1D << 1;
+    
+    char cmd[2];
+        
+    cmd[0] = 0x0D;
+    i2c.write(addr, cmd, 1, true);
+    i2c.read(addr, cmd, 1);        
+        
+    pc.printf("Reponse = 0x%02x \r\n",cmd[0]);
+    pc.printf((cmd[0] == 0x2a)?"Everything is working fine!":"Oopps, it seems there is an issue...");
+    
+    cmd[1] = 0x01;
+    i2c.write(addr, cmd, 2);
+    
+    cmd[0] = 0x0F;
+    cmd[1] = 0x30;
+    i2c.write(addr, cmd, 2);
+    
+    cac(0x7A);
+    cac(0xFF);
+    cac(0x76);
+    cac(0x77);
+    cac(0x02);
+        
+    int his[20] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+    int length = 20;
+    int cpt  = 0;
+        
+    while(1) {        
+        char acc[2];
+        cmd[0] = 0x05;
+        i2c.write(addr, cmd, 1, true);
+        i2c.read(addr, acc, 2);
+        
+        // Si négatif, propagation du bit de signe
+        int z = acc[0] > 127 ? 0xFFFFF000 | (acc[0] << 4) : acc[0] << 4;
+        
+        // Ajout des LSB
+        z += acc[1] >> 4;
+        
+        z = z > 2047 ? 2048 - z : z;
+        z = z > 1024 ? 1024 : z;
+        z = z < -1024 ? -1024 : z;
+        
+        pc.printf("%d \r\n", z);
+                
+        int angle = acos((double)abs(z)/1024.0) * 5729; //5729 = 180 / pi * 100
+        his[cpt] = angle;
+        cpt++;
+        cpt %= length;
+        
+        angle = 0;
+        for (int i = 0; i < length; i++) angle += his[i];
+        
+        angle /= length;
+                
+        pc.printf("Angle : %d\r\n", angle);
+        
+        sendCharToSS((char)(angle / 1000));
+        sendCharToSS((char)((angle / 100) % 10));
+        sendCharToSS((char)((angle / 10) % 10));
+        sendCharToSS((char)(angle % 10));
+        
+        pc.printf("--------- \r\n");
+
+        wait(0.025);
+    }
+}