Issue #1671: implement gyroscope rotation measurement for CalliopeV3

OpenRobertaServer/src/test/resources/crossCompilerTests/_expected/robotSpecific/astGenerated/calliopev3/sensor_gyro.ast

@@ -0,0 +1,16 @@
+SerialWriteAction[value: StringConst[value: Gyro test. Press A to step through]]
+
+(repeat [FOREVER, BoolConst[value: true]]
+AktionStmt [SerialWriteAction[value: SensorExpr [GyroSensor [_G, ANGLE, X]]]]WaitTimeStmt[time: NumConst[value: 100]]
+if SensorExpr [KeysSensor [A, PRESSED, - EMPTY_SLOT -]]
+,thenStmtFlowCon[flow: BREAK]
+
+)
+
+(repeat [FOREVER, BoolConst[value: true]]
+AktionStmt [SerialWriteAction[value: SensorExpr [GyroSensor [_G, ANGLE, Y]]]]WaitTimeStmt[time: NumConst[value: 100]]
+if SensorExpr [KeysSensor [A, PRESSED, - EMPTY_SLOT -]]
+,thenStmtFlowCon[flow: BREAK]
+
+)
+SerialWriteAction[value: StringConst[value: DONE]]

OpenRobertaServer/src/test/resources/crossCompilerTests/_expected/robotSpecific/collectorResults/calliopev3/sensor_gyro.txt

@@ -0,0 +1,6 @@
+Sensors: 
+[]
+Actors: 
+[UsedActor [, KEY]]
+Methods: 
+[GET_ROTATION]

OpenRobertaServer/src/test/resources/crossCompilerTests/_expected/robotSpecific/targetLanguage/calliopev3/sensor_gyro.py

@@ -0,0 +1,45 @@
+import calliopemini
+import random
+import math
+
+class BreakOutOfALoop(Exception): pass
+class ContinueLoop(Exception): pass
+
+timer1 = calliopemini.running_time()
+
+
+def run():
+    global timer1
+    print("Gyro test. Press A to step through")
+    while True:
+        print(get_rotation("pitch"))
+        calliopemini.sleep(100)
+        if calliopemini.button_a.is_pressed():
+            break
+    while True:
+        print(get_rotation("roll"))
+        calliopemini.sleep(100)
+        if calliopemini.button_a.is_pressed():
+            break
+    print("DONE")
+
+def main():
+    try:
+        run()
+    except Exception as e:
+        raise
+
+def get_rotation(dim):
+    x = -calliopemini.accelerometer.get_y()
+    y = calliopemini.accelerometer.get_x()
+    z = -calliopemini.accelerometer.get_z()
+    roll = math.atan2(y, z)
+    if (dim == "roll"):
+        return math.floor((-360*roll)/(2*math.pi))
+    elif (dim == "pitch"):
+        factor = y * math.sin(roll) + z * math.cos(roll)
+        if factor != 0:
+            pitch = math.atan(-x / (factor))            
+            return math.floor((360*pitch)/(2*math.pi))
+if __name__ == "__main__":
+    main()

RobotMbed/src/main/java/de/fhg/iais/roberta/visitor/CalliopeMethods.java

@@ -8,5 +8,6 @@ public enum CalliopeMethods {
     SET_BOTH_MOTORS,
     SET_MOTOR,
     SERVO_GET_ANGLE,
-    RECEIVE_MESSAGE
+    RECEIVE_MESSAGE,
+    GET_ROTATION
 }

RobotMbed/src/main/java/de/fhg/iais/roberta/visitor/codegen/CalliopeV3PythonVisitor.java

@@ -288,6 +288,14 @@ public Void visitInfraredSensor(InfraredSensor infraredSensor) {
 
     @Override
     public Void visitGyroSensor(GyroSensor gyroSensor) {
+        String slot = gyroSensor.getSlot();
+        if ( slot.equals("X") ) {
+            this.src.add(this.getBean(CodeGeneratorSetupBean.class).getHelperMethodGenerator().getHelperMethodName(CalliopeMethods.GET_ROTATION), "(\"pitch\")");
+        } else if ( slot.equals("Y") ) {
+            this.src.add(this.getBean(CodeGeneratorSetupBean.class).getHelperMethodGenerator().getHelperMethodName(CalliopeMethods.GET_ROTATION), "(\"roll\")");
+        } else {
+            throw new DbcException("Slot " + slot + " is not valid!");
+        }
         return null;
     }
 

RobotMbed/src/main/java/de/fhg/iais/roberta/visitor/validate/CalliopeV3ValidatorAndCollectorVisitor.java

@@ -29,7 +29,7 @@ public CalliopeV3ValidatorAndCollectorVisitor(
 
     @Override
     public Void visitGyroSensor(GyroSensor gyroSensor) {
-        addErrorToPhrase(gyroSensor, "BLOCK_NOT_SUPPORTED");
+        usedMethodBuilder.addUsedMethod(CalliopeMethods.GET_ROTATION);
         return null;
     }
 

RobotMbed/src/main/resources/calliope.methods.yml

@@ -128,4 +128,4 @@ GET_ROTATION:
             factor = y * math.sin(roll) + z * math.cos(roll)
             if factor != 0:
                 pitch = math.atan(-x / (factor))            
-                return math.floor((360*pitch)/(2*math.pi))
+                return math.floor((360*pitch)/(2*math.pi))

RobotMbed/src/main/resources/calliopeV3/program.toolbox.expert.xml

@@ -206,6 +206,7 @@
             <field name="MODE">DIGITAL</field>
         </block>
         <block type="robSensors_rssi_getSample"></block>
+        <block type="robSensors_gyro_getSample"></block>
         <block type="robSensors_accelerometer_getSample"></block>
         <block type="robSensors_humidity_getSample"></block>
         <block type='robSensors_moisture_getSample'></block>