Diff: main.cpp

Revision:

0:37bb1a446e69

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Oct 10 04:48:19 2021 +0000
@@ -0,0 +1,51 @@
+#include "mbed.h"
+
+#define PI 3.1415
+
+AnalogIn volume(A1);
+I2C i2c(D4, D5);        // sda, scl
+Serial pc(USBTX, USBRX, 115200); // tx, rx
+
+int id_list[6] = {
+    0x03, 0x06, 0x14, 0x15, 0x19, 0x20
+};
+ 
+int TxAngleI2C(int id_device, int position){
+    char cmd[4] = {0xFF, 0x00, 0x00, 0xFF};
+    cmd[1] = id_device;
+    cmd[2] = position;
+    
+    wait_ms(10);
+    int flag_send = i2c.write(id_device, cmd, 4);
+    
+    if(flag_send == 0){
+        return 0;
+    }else{
+        return 1;
+    }
+}
+
+int GetJoyAngle(){
+    float dat_v = volume.read();
+    
+    int angle_24p = int(dat_v * 24);
+    
+    pc.printf("%d\r\n", angle_24p);
+    
+    return angle_24p;
+}
+ 
+int main() {
+    
+    pc.printf("OK\r\n");
+    
+    while(1) {
+        TxAngleI2C(0x03, GetJoyAngle()+18);
+        TxAngleI2C(0x06, GetJoyAngle()+12);
+        TxAngleI2C(0x14, GetJoyAngle()+18);
+        TxAngleI2C(0x15, GetJoyAngle()+18);
+        TxAngleI2C(0x19, GetJoyAngle()+6);
+        TxAngleI2C(0x20, GetJoyAngle()+6);
+        wait(0.01);
+    }
+}
\ No newline at end of file