From 8999977fc58a1edf35f9ef2f051f62bb48ae9d16 Mon Sep 17 00:00:00 2001 From: Jim Martens Date: Tue, 9 Jun 2015 17:10:11 +0200 Subject: [PATCH] [ES] Blatt 3 fertiggestellt Signed-off-by: Jim Martens --- es/blatt3/uebung3-1/uebung3-1.ino | 8 +- es/blatt3/uebung3-3/uebung3-3.ino | 197 +++++++++++++++--------------- es/blatt3/uebung3-4/uebung3-4.ino | 46 ++----- es/blatt3/uebung3-5/uebung3-5.ino | 112 ++++++++++------- 4 files changed, 177 insertions(+), 186 deletions(-) diff --git a/es/blatt3/uebung3-1/uebung3-1.ino b/es/blatt3/uebung3-1/uebung3-1.ino index 620ad5b..523ff9a 100644 --- a/es/blatt3/uebung3-1/uebung3-1.ino +++ b/es/blatt3/uebung3-1/uebung3-1.ino @@ -4,8 +4,8 @@ int az = 50; int xout = A1; -int zout = A3; -int vref = A0; +int zout = A2; +int vref = A3; // used to achieve a 10 Hz frequency // don't touch them @@ -55,8 +55,8 @@ void loop() { double differenceXRef = xAxis - ref; double differenceZRef = zAxis - ref; - double drehrateX = differenceXRef / 9.1; - double drehrateZ = differenceZRef / 9.1; + double drehrateX = ((differenceXRef * 5000) / 1024) / 9.1; + double drehrateZ = ((differenceZRef * 5000) / 1024) / 9.1; Serial.print("x:"); Serial.println(xAxis); diff --git a/es/blatt3/uebung3-3/uebung3-3.ino b/es/blatt3/uebung3-3/uebung3-3.ino index 24c6b00..ee7e46c 100644 --- a/es/blatt3/uebung3-3/uebung3-3.ino +++ b/es/blatt3/uebung3-3/uebung3-3.ino @@ -1,135 +1,130 @@ #include - // these variables describe the used hardware pins // adjust them when you use other pins // hardware pins int az = 50; - int xout = A4; int zout = A2; int vref = A3; int ledPin = 13; - // servo Servo ourServo; int servoPin = 11; - // used to achieve a 10 Hz frequency // don't touch them long rc = 1049999; - // flags for servo bool volatile cwMaxReached = false; bool volatile ccwMaxReached = false; bool volatile lightLED = false; - bool volatile read_ready = false; +// tmp variables +int volatile zAxis = 0; +int volatile ref = 0; +double volatile differenceZRef = 0; +double volatile rotationZ = 0; +int volatile currentServoPos = 0; +int volatile newServoPos = 0; + /** - * Calculates the servo position. - * - * @param int currentServoPos - * @param int rotationZ - */ +* Calculates the servo position. +* +* @param int currentServoPos +* @param int rotationZ +*/ int calculate_new_servo_pos(int currentServoPos, int rotationZ) { - int newServoPos = currentServoPos + rotationZ; - - if (newServoPos > 159) { - newServoPos = 159; - cwMaxReached = true; - lightLED = true; - } - - if (newServoPos < 25) { - newServoPos = 25; - ccwMaxReached = true; - lightLED = true; - } - - return newServoPos; + int newServoPos = currentServoPos + rotationZ; + // faktor 10 zu gross (weil messung grad/sec + + if (newServoPos > 159) { + newServoPos = 159; + cwMaxReached = true; + lightLED = true; + } + if (newServoPos < 25) { + newServoPos = 25; + ccwMaxReached = true; + lightLED = true; + } + return newServoPos; } - /** - * Setup function for initial setup code - */ +* 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); - - // initialize serial port - Serial.begin(9600); + 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); + // initialize serial port + Serial.begin(9600); } - /** - * Loop function for main code - */ +* Loop function for main code +*/ void loop() { - if (read_ready) { - int zAxis = analogRead(zout); - int ref = analogRead(vref); - - double differenceZRef = zAxis - ref; - double rotationZ = differenceZRef / 9.1; - - Serial.print("rotationZ: "); - Serial.println(rotationZ); - - int currentServoPos = ourServo.read(); - int newServoPos = calculate_new_servo_pos(currentServoPos, rotationZ); - - Serial.print("currentServoPos: "); - Serial.println(currentServoPos); - - Serial.print("newServoPos: "); - Serial.println(newServoPos); - - if (lightLED) { - digitalWrite(ledPin, HIGH); - delay(10); - digitalWrite(ledPin, LOW); - delay(10); - digitalWrite(ledPin, HIGH); - delay(10); - digitalWrite(ledPin, LOW); - delay(10); - digitalWrite(ledPin, HIGH); - delay(10); - digitalWrite(ledPin, LOW); - lightLED = false; - } - ourServo.write(newServoPos + 1); - read_ready = false; - } + if (read_ready) { + + // berechnung auf 5000/1024 umstellen - was hat volt mit der berechnung zu tun? warscheinlich steht das so im aufgabenblatt ? + Serial.print("rotationZ: "); + Serial.println(rotationZ); + + Serial.print("currentServoPos: "); + Serial.println(currentServoPos); + Serial.print("newServoPos: "); + Serial.println(newServoPos); + if (lightLED) { + blink(); + } + ourServo.write(newServoPos + 1); + read_ready = false; + } } - /** - * Used to handle the timer. - */ +* Used to handle the timer. +*/ void TC1_Handler() { - // request static for some magic behind the curtain - TC_GetStatus(TC0, 1); - read_ready = true; + // request static for some magic behind the curtain + TC_GetStatus(TC0, 1); + read_ready = true; + zAxis = analogRead(zout); + ref = analogRead(vref); + differenceZRef = zAxis - ref; + differenceZRef = (differenceZRef * 5000) / 1024; + rotationZ = (differenceZRef / 9.1); + if (fabs(rotationZ) > 5) { + currentServoPos = ourServo.read(); + newServoPos = calculate_new_servo_pos(currentServoPos, rotationZ / 10); + } +} + +void blink(){ + digitalWrite(ledPin, HIGH); + delay(10); + digitalWrite(ledPin, LOW); + delay(10); + digitalWrite(ledPin, HIGH); + delay(10); + digitalWrite(ledPin, LOW); + delay(10); + digitalWrite(ledPin, HIGH); + delay(10); + digitalWrite(ledPin, LOW); + lightLED = false; } diff --git a/es/blatt3/uebung3-4/uebung3-4.ino b/es/blatt3/uebung3-4/uebung3-4.ino index eafdb54..fb9e640 100644 --- a/es/blatt3/uebung3-4/uebung3-4.ino +++ b/es/blatt3/uebung3-4/uebung3-4.ino @@ -24,31 +24,7 @@ bool volatile ccwMaxReached = false; bool volatile lightLED = false; bool volatile read_ready = false; - - -/** - * Calculates the servo position. - * - * @param int currentServoPos - * @param int rotationZ - */ -int calculate_new_servo_pos(int currentServoPos, int rotationZ) { - int newServoPos = currentServoPos + rotationZ; - - if (newServoPos > 159) { - newServoPos = 159; - cwMaxReached = true; - lightLED = true; - } - - if (newServoPos < 25) { - newServoPos = 25; - ccwMaxReached = true; - lightLED = true; - } - - return newServoPos; -} + /** * Setup function for initial setup code @@ -88,25 +64,17 @@ 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'; + String command; + command = Serial.readString(); + String ledOn = "LED_on"; + String ledOff = "LED_off"; - if (strcmp(command, "LED_on") == 0) { + if (command == ledOn) { digitalWrite(ledPin, HIGH); } - else { + else if (command == ledOff) { digitalWrite(ledPin, LOW); } - Serial.println(command); } } diff --git a/es/blatt3/uebung3-5/uebung3-5.ino b/es/blatt3/uebung3-5/uebung3-5.ino index 6eec18a..ca5b76f 100644 --- a/es/blatt3/uebung3-5/uebung3-5.ino +++ b/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()"); + 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; +}