Commit 88d4fce7 authored by Lisa Marie Dreier's avatar Lisa Marie Dreier
Browse files

neue Main, Aenderungen am ServoMotor

parent 73b35e21
......@@ -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) {
......
......@@ -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;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment