Cleanup

CMakeLists.txt

@@ -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

inc/ADC.h

@@ -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
 }

inc/DCMotor.h

@@ -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
 }

inc/ServoMotor.h

@@ -4,8 +4,6 @@
 
 #define SERVO_0 0
 #define SERVO_1 1
-#define SERVO_2 2
-#define SERVO_3 3
 
 #ifdef __cplusplus
 extern "C" {

inc/Util.h

 #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
 }

main.c

 #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) {

riddles/easy_riddle.h

+#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

riddles/spring_riddle.h

+#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

src/ADC.cpp

@@ -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);

src/DCMotor.cpp

@@ -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));
     }
 }

src/ExternButton.cpp

@@ -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;
 }

src/ServoMotor.cpp

@@ -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);

src/Util.cpp

 #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;
 }