rotate cmd ok

rem: ucPack dep

Author: eigen-value

examples/firmware_i2c/firmware_i2c.ino

@@ -94,39 +94,37 @@ void publishImu() {
     sendMessage(packeter.msg, msg_size);
 }
 
-void parseMessage() {
+void getData(size_t size) {
+
+    for (size_t i=0; i<size; i++){
+        data[i]=alvik.ext_wire->read();
+    }
+
+}
 
+void rotateCmd() {
     float angle;
-    uint8_t code;
+    size_t sz = sizeof(angle);
+    getData(sz);
+    memcpy(&angle, data, sz);
+
+    alvik.rotate(angle);
+}
+
+void parseMessage() {
 
     switch (command){
         case 'R':
-            // for (size_t i = 0; i < 9; i++) {
-            //     packeter.buffer.push(alvik.ext_wire->read());
-            // }
-            data[0]=alvik.ext_wire->read();
-            data[1]=alvik.ext_wire->read();
-            data[2]=alvik.ext_wire->read();
-            data[3]=alvik.ext_wire->read();
-            memcpy(&angle, data, 4);
-
-            // packeter.unpacketC1F(code, angle);
-            // alvik.rotate(angle);
-            alvik.serial->println(angle);
-            if (angle > 0) {
-                alvik.setLedRight(COLOR_VIOLET);
-            } else {
-                alvik.setLedRight(COLOR_RED);
-            }
-            //Serial.println("rotate");
+            rotateCmd();
+            break;
+        default:
             break;
     }
 
 }
 
 void receiveEvent(int event){
     command=alvik.ext_wire->read();
-    //Serial.println("requested\t"+String(command));
     parseMessage();
 
 }

examples/firmware_i2c/unoq/AlvikI2C.h

@@ -3,7 +3,6 @@
 #define ARDUINO_ALVIKCARRIER_ALVIKI2C_H
 
 #include <Wire.h>
-#include <ucPack.h>
 #define wire Wire1
 
 #define ALVIK_I2C_ADDRESS 0x2B
@@ -37,6 +36,17 @@ class AlvikI2C{
         wire.readBytes(version, 3);
     }
 
+    void rotate(float angle) {
+        int msg_size = sizeof(angle);
+        uint8_t msg[msg_size];
+
+        wire.beginTransmission(address);
+        wire.write('R');
+        memcpy(msg, &angle, sizeof(angle));
+        wire.write(msg, sizeof(angle));
+        wire.endTransmission();
+    }
+
     String getVersionString() {
         String versionStr = String(version[0]) + "." +
                             String(version[1]) + "." +

examples/firmware_i2c/unoq/unoq.ino

@@ -4,11 +4,6 @@
 
 AlvikI2C alvik;
 
-int msg_size;
-
-float f = 90;
-uint8_t msg[100];
-
 void setup() {
 
     alvik.begin();
@@ -25,40 +20,17 @@ void setup() {
 
 }
 
-void print_msg(size_t size) {
-
-    for (size_t i = 0; i < size; i++) {
-        Monitor.print(msg[i], HEX);
-        Monitor.print(" ");
-    }
-    Monitor.println("");
-
-}
-
 void loop() {
 
     Monitor.println(alvik.getVersionString());
     delay(1000);
 
-//    f = 90.0;
-//    wire.beginTransmission(address);
-//    wire.write('R');
-//    //msg_size = packeter.packetC1F('R', f);
-//    memcpy(msg, &f, sizeof(f));
-//    //wire.write(packeter.msg, msg_size);
-//    wire.write(msg, sizeof(f));
-//    wire.endTransmission();
-//    delay(1000);
-//
-//    f = -90.0;
-//    wire.beginTransmission(address);
-//    wire.write('R');
-//    //msg_size = packeter.packetC1F('R', f);
-//    memcpy(msg, &f, sizeof(f));
-//    //wire.write(packeter.msg, msg_size);
-//    wire.write(msg, sizeof(f));
-//    wire.endTransmission();
-//    delay(1000);
+    alvik.rotate(10.0);
+    delay(1000);
+
+    alvik.rotate(-10.0);
+    delay(1000);
+
 //
 //    wire.beginTransmission(address);
 //    wire.write('I');