Commit 6cde354e authored by Werner Sembach's avatar Werner Sembach
Browse files

Cleanup

parent f215a10f
......@@ -17,6 +17,7 @@ include_directories(${PROJECT_SOURCE_DIR}/Drivers/STM32F4xx_HAL_Driver/Inc)
include_directories(${PROJECT_SOURCE_DIR}/Drivers/CMSIS/Include)
include_directories(${PROJECT_SOURCE_DIR}/Drivers/CMSIS/Device/ST/STM32F4xx/Include)
include_directories(${PROJECT_SOURCE_DIR}/inc)
include_directories(${PROJECT_SOURCE_DIR}/riddles)
include_directories(${OPENCM3_PATH}/include)
set(LIBEZS_SOURCES
......
......@@ -7,7 +7,7 @@ extern "C" {
#endif
void adc_init(void);
uint32_t adc_get(uint8_t channel);
unsigned int adc_get(uint8_t channel);
#ifdef __cplusplus
}
......
......@@ -11,7 +11,7 @@ extern "C" {
void dc_init(void);
void dc_set(uint8_t direction, uint8_t speed);
void dc_set_percent(uint8_t direction, double speed);
#ifdef __cplusplus
}
......
......@@ -4,8 +4,6 @@
#define SERVO_0 0
#define SERVO_1 1
#define SERVO_2 2
#define SERVO_3 3
#ifdef __cplusplus
extern "C" {
......
#pragma once
#include <stdint.h>
#define LOWER_BOUND 0
#define UPPER_BOUND 1
#ifdef __cplusplus
extern "C" {
#endif
......@@ -15,7 +20,7 @@ double get_degree_long(void);
double get_degree_max(void);
bool simulate_spring(void);
void block_spring(void);
bool block_spring(uint8_t direction, unsigned int position);
#ifdef __cplusplus
}
......
#include <cyg/hal/hal_arch.h>
#include <cyg/kernel/kapi.h>
#include <stdint.h>
#include <stdio.h>
#include "ezs_io.h"
#include "DCMotor.h"
#include "ExternButton.h"
#include "ServoMotor.h"
#include "Util.h"
#include "spring_riddle.h"
//#include "easy_riddle.h"
#define STACKSIZE (CYGNUM_HAL_STACK_SIZE_MINIMUM+1024)
static cyg_uint8 my_stack[STACKSIZE];
static cyg_handle_t handle;
......@@ -19,36 +14,7 @@ static cyg_thread threaddata;
void main_thread(cyg_addrword_t arg) {
all_init();
bool button_pressed = false;
bool spring_broken = false;
bool upside_down = false;
servo_set_percent(SERVO_0, 0.0);
dc_set(0, 0);
while (true) {
//check if solved
if(button_pressed && spring_broken && upside_down) {
ezs_printf("CONGRATULATIONS! YOU FINALLY MADE IT!\n");
servo_set_percent(SERVO_0, 1.0);
break;
}
button_pressed = button_get();
double degree_max = get_degree_max();
upside_down = (degree_max > 90);
//spring riddle can only be solved if button is pressed
if(button_pressed) {
if (!spring_broken) {
spring_broken = simulate_spring();
}
}
else {
block_spring();
}
}
riddle();
}
void cyg_user_start(void) {
......
#pragma once
#include <stdint.h>
#include <stdio.h>
#include "ezs_io.h"
#include "DCMotor.h"
#include "ExternButton.h"
#include "ServoMotor.h"
#include "Util.h"
void riddle(void) {
while (1) {
if (button_get()) {
servo_set_percent(SERVO_0, 1.0);
}
else {
servo_set_percent(SERVO_0, 0.5);
}
}
}
\ No newline at end of file
#pragma once
#include <stdint.h>
#include <stdio.h>
#include <inttypes.h>
#include "ezs_io.h"
#include "DCMotor.h"
#include "ExternButton.h"
#include "ServoMotor.h"
#include "Util.h"
void riddle(void) {
bool button_pressed = false;
bool spring_broken = false;
bool upside_down = false;
servo_set_percent(SERVO_0, 0.5);
dc_set_percent(0, 0.0);
while (true) {
//poti protection
bool protection = block_spring(LOWER_BOUND, 256) && block_spring(UPPER_BOUND, 4096 - 256);
//check if solved
if(button_pressed && spring_broken && upside_down) {
ezs_printf("CONGRATULATIONS! YOU FINALLY MADE IT!\n");
servo_set_percent(SERVO_0, 1.0);
break;
}
button_pressed = button_get();
double degree_max = get_degree_max();
upside_down = (degree_max < 90);
//spring riddle can only be solved if button is pressed
if (!protection) {
if (spring_broken) {
dc_set_percent(0, 0.0);
}
else if (button_pressed) {
spring_broken = simulate_spring();
}
else if (!block_spring(UPPER_BOUND, 2048)) {
dc_set_percent(0, 0.0);
}
}
}
}
\ No newline at end of file
......@@ -56,7 +56,7 @@ extern "C" void adc_init() {
adc_power_on(ADC1);
}
extern "C" uint32_t adc_get(uint8_t channel) {
extern "C" unsigned int adc_get(uint8_t channel) {
uint8_t channel_array[16];
channel_array[0] = channel;
adc_set_regular_sequence(ADC1, 1, channel_array);
......
......@@ -2,6 +2,8 @@
#include "DCMotor.h"
#include <stdio.h>
#include <cyg/hal/hal_arch.h>
#include <libopencm3/cm3/common.h>
......@@ -126,13 +128,17 @@ void dc_init() {
CYGHWR_HAL_STM32_GPIO_SET(GPIO_B1);
}
void dc_set(uint8_t direction, uint8_t speed) {
void dc_set_percent(uint8_t direction, double speed) {
if (speed > 1.0 || speed < 0.0) {
printf("Error: dc_set_percent only accepts values between 0.0 and 1.0\n");
return;
};
if (direction == DC_RIGHT) {
timer_set_oc_value(TIM3, TIM_OC4, 0);
timer_set_oc_value(TIM3, TIM_OC3, (uint32_t)(speed * PWM_PERIOD / 255));
timer_set_oc_value(TIM3, TIM_OC3, (uint32_t)(speed * PWM_PERIOD));
}
else if (direction == DC_LEFT) {
timer_set_oc_value(TIM3, TIM_OC3, 0);
timer_set_oc_value(TIM3, TIM_OC4, (uint32_t)(speed * PWM_PERIOD / 255));
timer_set_oc_value(TIM3, TIM_OC4, (uint32_t)(speed * PWM_PERIOD));
}
}
......@@ -3,11 +3,11 @@
#include <cyg/hal/hal_arch.h>
extern "C" void button_init() {
CYGHWR_HAL_STM32_GPIO_SET(CYGHWR_HAL_STM32_PIN_IN(D, 11, PULLUP));
CYGHWR_HAL_STM32_GPIO_SET(CYGHWR_HAL_STM32_PIN_IN(D, 15, PULLUP));
}
extern "C" bool button_get() {
int button;
CYGHWR_HAL_STM32_GPIO_IN(CYGHWR_HAL_STM32_PIN_IN(D, 11, PULLUP), &button);
CYGHWR_HAL_STM32_GPIO_IN(CYGHWR_HAL_STM32_PIN_IN(D, 15, PULLUP), &button);
return !(bool)button;
}
......@@ -11,10 +11,8 @@
#include <libopencm3/stm32/f4/rcc.h>
#include <libopencm3/stm32/f4/gpio.h>
#define GPIO_D12 CYGHWR_HAL_STM32_PIN_ALTFN_OUT(D,12, 2, PUSHPULL,NONE,LOW)
#define GPIO_D13 CYGHWR_HAL_STM32_PIN_ALTFN_OUT(D,13, 2, PUSHPULL,NONE,LOW)
#define GPIO_D14 CYGHWR_HAL_STM32_PIN_ALTFN_OUT(D,14, 2, PUSHPULL,NONE,LOW)
#define GPIO_D15 CYGHWR_HAL_STM32_PIN_ALTFN_OUT(D,15, 2, PUSHPULL,NONE,LOW)
// The timer ticks per microsecond. Can be adjusted by measuring PWM on an oscilloscope.
#define PWM_TIMER_TICKS_PER_MICROSECOND 84
......@@ -109,47 +107,39 @@ static void pwm_setup() {
rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_IOPDEN);
// AF = alternate function
gpio_mode_setup(GPIOD, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO12 | GPIO13 | GPIO14 | GPIO15);
gpio_mode_setup(GPIOD, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO13 | GPIO14);
// AF2 = TIM4_CH1..4
gpio_set_af(GPIOD, GPIO_AF2, GPIO12 | GPIO13 | GPIO14 | GPIO15);
gpio_set_af(GPIOD, GPIO_AF2, GPIO13 | GPIO14);
timer_enable_counter(TIM4);
}
extern "C" void servo_init() {
pwm_setup();
CYGHWR_HAL_STM32_GPIO_SET(GPIO_D12);
CYGHWR_HAL_STM32_GPIO_SET(GPIO_D13);
CYGHWR_HAL_STM32_GPIO_SET(GPIO_D14);
CYGHWR_HAL_STM32_GPIO_SET(GPIO_D15);
}
extern "C" void servo_set_percent(uint8_t servo, double position) {
if (position > 1.0) {
printf("Error: servo_set_percent only accepts values between 0.0 and 1.0");
if (position > 1.0 || position < 0.0) {
printf("Error: servo_set_percent only accepts values between 0.0 and 1.0\n");
return;
};
switch (servo) {
case SERVO_0:
timer_set_oc_value(TIM4, TIM_OC1, (uint32_t)(MIN_UPTIME * PWM_TIMER_TICKS_PER_MICROSECOND + (position * MAX_UPTIME * PWM_TIMER_TICKS_PER_MICROSECOND)));
break;
case SERVO_1:
timer_set_oc_value(TIM4, TIM_OC2, (uint32_t)(MIN_UPTIME * PWM_TIMER_TICKS_PER_MICROSECOND + (position * MAX_UPTIME * PWM_TIMER_TICKS_PER_MICROSECOND)));
break;
case SERVO_2:
case SERVO_1:
timer_set_oc_value(TIM4, TIM_OC3, (uint32_t)(MIN_UPTIME * PWM_TIMER_TICKS_PER_MICROSECOND + (position * MAX_UPTIME * PWM_TIMER_TICKS_PER_MICROSECOND)));
break;
case SERVO_3:
timer_set_oc_value(TIM4, TIM_OC4, (uint32_t)(MIN_UPTIME * PWM_TIMER_TICKS_PER_MICROSECOND + (position * MAX_UPTIME * PWM_TIMER_TICKS_PER_MICROSECOND)));
break;
default:
break;
}
}
extern "C" void servo_set_degree(uint8_t servo, double position) {
if (position > 180) {
printf("Error: servo_set_degree only accepts values between 0 and 180");
if (position > 180 || position < 0) {
printf("Error: servo_set_degree only accepts values between 0 and 180\n");
return;
};
servo_set_percent(servo, position / 180.0);
......
#include "Util.h"
#include <math.h>
#include <inttypes.h>
#include "stm32f411e_discovery.h"
#include "stm32f411e_discovery_gyroscope.h"
......@@ -93,38 +94,48 @@ extern "C" double get_degree_max() {
}
//returns true if spring is in "broken" state
// TODO: make parameters possible in a cool way
// get array with (thresold, direction, power, solved)
extern "C" bool simulate_spring() {
uint32_t adc_value = adc_get(11);
if (adc_value < 256) {
//potty protection deadzone
dc_set(0, 0);
}
else if(adc_value > (4096 - 256)) {
//spring is "broken"
dc_set(0, 0);
return true;
}
else {
//main simulation
dc_set(DC_RIGHT, (uint8_t)(adc_value / 4096.0 * 255));
unsigned int adc_value = adc_get(11);
if(!block_spring(LOWER_BOUND, 256) && !block_spring(UPPER_BOUND, 4096-256)) {
if (adc_value > (4096 - 768)) {
//spring is "broken"
dc_set_percent(0, 0.0);
return true;
}
else if (adc_value > 768) {
//main simulation
dc_set_percent(DC_RIGHT, (adc_value - 768.0) / (4096.0 - 1536));
}
else {
//lower deadzone
dc_set_percent(0, 0.0);
}
}
return false;
}
// turns the motor real fast if in certain position
// TODO: make that position a parameter
extern "C" void block_spring() {
//blocks sping by giving full power on motor in certain positions
extern "C" bool block_spring(uint8_t direction, unsigned int position) {
uint32_t adc_value = adc_get(11);
if(adc_value > 2048) {
dc_set(DC_RIGHT, 255);
}
else if(adc_value > (2048 - 256)) {
dc_set(DC_RIGHT, (uint8_t)(adc_value - (2048 - 256) / 256.0 * 255));
if (direction == UPPER_BOUND) {
if (adc_value > position) {
dc_set_percent(DC_RIGHT, 1.0);
} else if (adc_value > (position - 256)) {
dc_set_percent(DC_RIGHT, (adc_value - (position - 256.0)) / 256.0);
} else {
return false;
}
}
else {
dc_set(0, 0);
if (adc_value < position) {
dc_set_percent(DC_LEFT, 1.0);
} else if (adc_value < (position + 256)) {
dc_set_percent(DC_LEFT, ((position + 256.0) - adc_value) / 256.0);
} else {
return false;
}
}
return true;
}
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