[ES] Blatt 3 fertiggestellt

Signed-off-by: Jim Martens <github@2martens.de>

es/blatt3/uebung3-5/uebung3-5.ino

@@ -5,6 +5,9 @@
 // hardware pins
 int az = 50;
 
+int xout = A4;
+int zout = A2;
+int vref = A3;
 int ledPin = 13;
 
 // servo
@@ -24,53 +27,71 @@ bool volatile read_ready = false;
 
 
 /**
- * Validates the new servo position.
+ * Validates the servo position.
  *
- * @param long newServoPos
+ * @param int newServoPos
  */
-int validate_new_servo_pos(long newServoPos) {
-
+int validate_new_servo_pos(int newServoPos) {
     if (newServoPos > 159) {
         newServoPos = 159;
         cwMaxReached = true;
         lightLED = true;
+        Serial.println("Interval 25-159");
     }
 
     if (newServoPos < 25) {
         newServoPos = 25;
         ccwMaxReached = true;
         lightLED = true;
+        Serial.println("Interval 25-159");
     }
 
-    int validatedServoPos = (int) newServoPos;
-
-    return validatedServoPos;
+    return newServoPos;
 }
 
-/**
- * Parses the angle of the moveTo command.
- *
- * @param char command
- */
-long parse_angle(char* command) {
-    char* angle;
+int parse_angle(String command) {
+  int indexP1 = command.indexOf('(');
+  int indexP2 = command.indexOf(')');
+  String command2 = command;
+  command2.remove(indexP1);
+  if (command2 != "moveTo") {
+    Serial.println("Use moveTo(<angle>)");
+    return ourServo.read();
+  }
+  
+  command.remove(indexP2);
+  int angle = command.substring(indexP1 + 1).toInt();
 
-    angle = strtok(command, "()");
-    angle = strtok(NULL, "()");
-
-    return strtol(angle, NULL, 10);
+  return angle;
 }
+  
 
 /**
  * Setup function for initial setup code
  */
 void setup() {
+    pmc_set_writeprotect(false);
+    pmc_enable_periph_clk(ID_TC1);
+
+    // configure hardware timer
+    TC_Configure(TC0, 1, TC_CMR_WAVE | TC_CMR_WAVSEL_UP_RC | TC_CMR_TCCLKS_TIMER_CLOCK2) ;
+    TC_SetRC(TC0, 1, rc);
+
+    TC0->TC_CHANNEL[1].TC_IER=TC_IER_CPCS;   // IER = interrupt enable register
+    TC0->TC_CHANNEL[1].TC_IDR=~TC_IER_CPCS;
+
+    NVIC_ClearPendingIRQ(TC1_IRQn);
+    NVIC_EnableIRQ(TC1_IRQn);
+
+    // start hardware timer
+    //TC_Start(TC0, 1);
+
     // Configure pins
     pinMode(ledPin, OUTPUT);
     pinMode(az, OUTPUT);
     digitalWrite(az, HIGH);
     digitalWrite(ledPin, LOW);
-
+    
     ourServo.attach(servoPin);
     ourServo.write(90);
 
@@ -83,28 +104,35 @@ void setup() {
  */
 void loop() {
     if (Serial.available() > 0) {
-        char command[8];
-        char currentChar;
-        int i = 0;
-        bool readable = true;
-        while (readable) {
-            currentChar = Serial.read();
-            readable = (currentChar != -1);
-            command[i] = currentChar;
-            i++;
-        }
-        command[i] = '\0';
-
-        long servoPos = parse_angle(command);
-        int validatedServoPos = validate_new_servo_pos(servoPos);
-
-        ourServo.write(validatedServoPos);
-
-        if (lightLED) {
-            lightLED = false;
-            digitalWrite(ledPin, HIGH);
-            delay(500);
-            digitalWrite(ledPin, LOW);
-        }
+      String command;
+      char currentChar;
+      int i = 0;
+      bool readable = true;
+      command = Serial.readString();
+      
+      int servoPos = parse_angle(command);
+      int validatedServoPos = validate_new_servo_pos(servoPos);
+      
+      Serial.print("move to angle ");
+      Serial.println(validatedServoPos);
+      
+      ourServo.write(validatedServoPos);
+      
+      if (lightLED) {
+        lightLED = false;
+        digitalWrite(ledPin, HIGH);
+        delay(500);
+        digitalWrite(ledPin, LOW);
+      }
     }
 }
+
+/**
+ * Used to handle the timer.
+ */
+void TC1_Handler()
+{
+    // request static for some magic behind the curtain
+    TC_GetStatus(TC0, 1);
+    read_ready = true;
+}