neue Main, Aenderungen am ServoMotor

main.c

@@ -5,6 +5,7 @@
 #include <stdio.h>
 #include <math.h>
 #include <stm32f4/include/stm32f4xx_hal_conf.h>
+#include <math.h>
 
 #include "stm32f411e_discovery.h"
 #include "stm32f411e_discovery_gyroscope.h"
@@ -116,9 +117,118 @@ void get_degree() {
 
 #define PWM_TIMER_TICKS_PER_MICROSECOND 84
 
+double get_max_degree() {
+    int16_t xyz_accelero[3];
+    BSP_ACCELERO_GetXYZ(xyz_accelero);
+
+    double neigung_zu_kurz_tan = 0;
+    double neigung_zu_lang_tan = 0;
+
+    if (xyz_accelero[2] != 0) {
+        // /1000 for more stability
+        neigung_zu_kurz_tan = (double) (xyz_accelero[1] / 1000) / (double) (xyz_accelero[2] / 1000);
+        neigung_zu_lang_tan = (double) (xyz_accelero[0] / 1000) / (double) (xyz_accelero[2] / 1000);
+    }
+
+    double neigung_zu_kurz_rad = atan(neigung_zu_kurz_tan);
+    double neigung_zu_lang_rad = atan(neigung_zu_lang_tan);
+
+    if (xyz_accelero[2] <= 0 && xyz_accelero[1] >= 0) {
+        neigung_zu_kurz_rad += M_PI;
+    } else if (xyz_accelero[2] <= 0 && xyz_accelero[1] < 0) {
+        neigung_zu_kurz_rad -= M_PI;
+    }
+
+    if (xyz_accelero[2] <= 0 && xyz_accelero[0] >= 0) {
+        neigung_zu_lang_rad += M_PI;
+    } else if (xyz_accelero[2] <= 0 && xyz_accelero[0] < 0) {
+        neigung_zu_lang_rad -= M_PI;
+    }
+
+    double neigung_zu_kurz_deg = neigung_zu_kurz_rad / M_PI * 180.;
+    double neigung_zu_lang_deg = neigung_zu_lang_rad / M_PI * 180.;
+
+    //ezs_printf("Neigung zu kurzer Seite: tan %6.1f rad %6.1fπ deg %6.1f°\n", neigung_zu_kurz_tan, neigung_zu_kurz_rad / M_PI, neigung_zu_kurz_deg);
+    //ezs_printf("Neigung zu langen Seite: tan %6.1f rad %6.1fπ deg %6.1f°\n", neigung_zu_lang_tan, neigung_zu_lang_rad / M_PI, neigung_zu_lang_deg);
+    if(neigung_zu_kurz_deg > neigung_zu_lang_deg){
+        return neigung_zu_kurz_deg;
+    }
+    else{
+        return neigung_zu_lang_deg;
+    }
+}
+
+//gibt true zurueck, falls das Zahnrad ueber den schwersten Punkt gedreht wurde
+bool motor_on (uint16_t input_adc){
+    //uint16_t input_adc = read_adc_naiive(11);
+    ezs_printf("ADC: %5d    ", input_adc);
+
+    if (input_adc < 500) {
+        ezs_printf("Speed: %3d\n", 0);
+        dc_set(0, 0);
+    }
+    else if(input_adc > 3500 && input_adc < 3900) {
+        ezs_printf("Speed: %3d\n", 0);
+        dc_set(0, 0);
+        return true;
+    }
+    else if(input_adc > 3900) {
+        ezs_printf("Speed: %3d\n", 0);
+        dc_set(DC_LEFT, 250);
+        return true;
+    }
+    else {
+        uint8_t speed = input_adc / 4096.0 * 125;
+        ezs_printf("Speed: %3d\n", speed);
+        dc_set(DC_LEFT, speed);
+    }
+    return false;
+}
+
 void test_thread(cyg_addrword_t arg) {
     test_init();
+    bool button_state = false;
+    bool ueberdreht = false;
+    bool upside_down = false;
+    uint16_t input_adc;
+    
+    servo_set(SERVO_0, 100);
+
+    while (1) {
+        input_adc = read_adc_naiive(11);
+        ezs_printf("adc: %3d\n", input_adc);
+        button_state = button_get();
+        double max_degree = get_max_degree();
+        
+        if(max_degree > 90)
+                upside_down = true;
+        else
+            upside_down = false;
+        
+        if(button_state) {
+            ueberdreht = motor_on(input_adc);
+        }
+        else {
+            if(input_adc > 2500) {
+                uint8_t speed = 250; //input_adc / 4096.0 * 125;  //max: 106
+                ezs_printf("Speed: %3d\n", speed);
+                dc_set(DC_LEFT, speed);
+            }
+            else {
+                dc_set(0, 0);
+            }
+            ueberdreht = false;
+        }
+        
+        if(button_state && ueberdreht && upside_down)
+        {
+            //Problem solved
+            servo_set(SERVO_0, button_get()*200);
+            cyg_thread_delay(250);
+        }
+    }
 
+    /*
     while (1) {
         uint16_t input_adc = read_adc_naiive(11);
         ezs_printf("ADC: %5d    ", input_adc);
@@ -133,7 +243,7 @@ void test_thread(cyg_addrword_t arg) {
             dc_set(DC_LEFT, speed);
         }
     }
-
+    */
     /*int i = 0;
     while (!BSP_PB_GetState(BUTTON_KEY)) {
         for (i = 0; i < 255; i += 5) {

src/ServoMotor.cpp

@@ -118,16 +118,16 @@ extern "C" void servo_init() {
 extern "C" void servo_set(uint8_t servo, uint8_t position) {
     switch (servo) {
         case SERVO_0:
-            timer_set_oc_value(TIM4, TIM_OC1, 125 * PWM_TIMER_TICKS_PER_MICROSECOND + (position * 375 * PWM_TIMER_TICKS_PER_MICROSECOND / 255));
+            timer_set_oc_value(TIM4, TIM_OC1, 150 * PWM_TIMER_TICKS_PER_MICROSECOND + (position * 350 * PWM_TIMER_TICKS_PER_MICROSECOND / 255));
             break;
         case SERVO_1:
-            timer_set_oc_value(TIM4, TIM_OC2, 125 * PWM_TIMER_TICKS_PER_MICROSECOND + (position * 375 * PWM_TIMER_TICKS_PER_MICROSECOND / 255));
+            timer_set_oc_value(TIM4, TIM_OC2, 150 * PWM_TIMER_TICKS_PER_MICROSECOND + (position * 350 * PWM_TIMER_TICKS_PER_MICROSECOND / 255));
             break;
         case SERVO_2:
-            timer_set_oc_value(TIM4, TIM_OC3, 125 * PWM_TIMER_TICKS_PER_MICROSECOND + (position * 375 * PWM_TIMER_TICKS_PER_MICROSECOND / 255));
+            timer_set_oc_value(TIM4, TIM_OC3, 150 * PWM_TIMER_TICKS_PER_MICROSECOND + (position * 350 * PWM_TIMER_TICKS_PER_MICROSECOND / 255));
             break;
         case SERVO_3:
-            timer_set_oc_value(TIM4, TIM_OC4, 125 * PWM_TIMER_TICKS_PER_MICROSECOND + (position * 375 * PWM_TIMER_TICKS_PER_MICROSECOND / 255));
+            timer_set_oc_value(TIM4, TIM_OC4, 150 * PWM_TIMER_TICKS_PER_MICROSECOND + (position * 350 * PWM_TIMER_TICKS_PER_MICROSECOND / 255));
             break;
         default:
             break;