From 0fda681119f80b9f04995a8e40eb46dec689e8ff Mon Sep 17 00:00:00 2001 From: Alwin Berger Date: Tue, 31 Jan 2023 10:37:37 +0100 Subject: [PATCH] add lift demo with states --- .../CORTEX_M3_MPS2_QEMU_GCC/FreeRTOSConfig.h | 1 + .../Demo/CORTEX_M3_MPS2_QEMU_GCC/Makefile | 6 + .../CORTEX_M3_MPS2_QEMU_GCC/init/startup.c | 5 +- FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/main.c | 4 + .../Demo/CORTEX_M3_MPS2_QEMU_GCC/main_lift.c | 182 +++++++++++ .../tacle/app/lift/README | 31 ++ .../tacle/app/lift/changeLog.txt | 46 +++ .../tacle/app/lift/lift.c | 132 ++++++++ .../tacle/app/lift/liftlibcontrol.c | 284 ++++++++++++++++++ .../tacle/app/lift/liftlibcontrol.h | 64 ++++ .../tacle/app/lift/liftlibio.c | 67 +++++ .../tacle/app/lift/liftlibio.h | 27 ++ 12 files changed, 847 insertions(+), 2 deletions(-) create mode 100644 FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/main_lift.c create mode 100755 FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/tacle/app/lift/README create mode 100755 FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/tacle/app/lift/changeLog.txt create mode 100755 FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/tacle/app/lift/lift.c create mode 100755 FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/tacle/app/lift/liftlibcontrol.c create mode 100755 FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/tacle/app/lift/liftlibcontrol.h create mode 100755 FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/tacle/app/lift/liftlibio.c create mode 100755 FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/tacle/app/lift/liftlibio.h diff --git a/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/FreeRTOSConfig.h b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/FreeRTOSConfig.h index 17b7c1d8..cec63a52 100644 --- a/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/FreeRTOSConfig.h +++ b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/FreeRTOSConfig.h @@ -122,4 +122,5 @@ extern void vLoggingPrintf( const char * pcFormatString, #define xPortGetFreeHeapSize ( x ) #endif +#define configUSE_TASK_NOTIFICATIONS 1 #endif /* FREERTOS_CONFIG_H */ diff --git a/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/Makefile b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/Makefile index 07595a7a..18b1f9ec 100644 --- a/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/Makefile +++ b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/Makefile @@ -83,6 +83,11 @@ ifeq ($(TACLE_DEMO), 1) SOURCE_FILES += main_tacle.c CFLAGS := -DmainCREATE_TACLE_DEMO=1 +else +ifeq ($(LIFT_DEMO), 1) + SOURCE_FILES += main_lift.c + + CFLAGS := -DmainCREATE_LIFT_DEMO=1 else SOURCE_FILES += main_blinky.c @@ -93,6 +98,7 @@ endif endif endif endif +endif DEFINES := -DQEMU_SOC_MPS2 -DHEAP3 diff --git a/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/init/startup.c b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/init/startup.c index 0c8ea57a..f620d9f2 100644 --- a/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/init/startup.c +++ b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/init/startup.c @@ -36,7 +36,7 @@ extern void xPortPendSVHandler( void ); extern void xPortSysTickHandler( void ); extern void uart_init(); extern int main(); -extern void isr_starter( void ); // main_tmr.c +// extern void isr_starter( void ); // main_tmr.c void __attribute__( ( weak ) ) EthernetISR( void ); @@ -177,7 +177,8 @@ const uint32_t * isr_vector[] __attribute__( ( section( ".isr_vector" ) ) ) = 0, /* reserved */ ( uint32_t * ) &xPortPendSVHandler, /* PendSV handler -2 */ ( uint32_t * ) &xPortSysTickHandler, /* SysTick_Handler -1 */ - ( uint32_t * ) &isr_starter, /* uart0 receive 0 */ + // ( uint32_t * ) &isr_starter, /* uart0 receive 0 */ + 0, 0, /* uart0 transmit */ 0, /* uart1 receive */ 0, /* uart1 transmit */ diff --git a/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/main.c b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/main.c index fc82230d..90112004 100644 --- a/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/main.c +++ b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/main.c @@ -83,6 +83,10 @@ int main() { main_tacle(); } + #elif ( mainCREATE_LIFT_DEMO == 1 ) + { + main_lift(); + } #else { #error "Invalid Selection...\nPlease Select a Demo application from the main command" diff --git a/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/main_lift.c b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/main_lift.c new file mode 100644 index 00000000..77cd8339 --- /dev/null +++ b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/main_lift.c @@ -0,0 +1,182 @@ +/* + * FreeRTOS V202111.00 + * Copyright (C) 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of + * this software and associated documentation files (the "Software"), to deal in + * the Software without restriction, including without limitation the rights to + * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of + * the Software, and to permit persons to whom the Software is furnished to do so, + * subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS + * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR + * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER + * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * + * https://www.FreeRTOS.org + * https://github.com/FreeRTOS + * + */ + +#include +#include +#include +#include +/* +TMR Demo with retry +prvSamplerTask will read 4 Bytes of Input into a buffer, unlocks xMutexInput +prvReplicateA and prvReplicateB wait on xMutexInput to average the Inputs and +sum up all numbers up to the Input. +ReplicateA will fail if mod 11 = 0, but only once +ReplicateB will fail if mod 12 = 0 +ReplicateC also exists and will never fail, does not run by default +Each Replicate outputs to it's own queue +prvVoterTask will wait on ReplicateA&B +If they disagree ReplicateC will be started by mutex. +If all the Replicates disagree now the sampler will be engaged once more +*/ +// include tacle benches +#include "tacle/app/lift/lift.c" // void lift_controller() +#include "tacle/app/lift/liftlibcontrol.c" +#include "tacle/app/lift/liftlibio.c" + +__attribute__((noinline)) static void trigger_Qemu_break( void ) +{ + lift_return(); + puts("Trigger"); + while (1) { + } +} + +// Begin Input Stuff +volatile unsigned char FUZZ_INPUT[4096] = {0x0}; +volatile uint32_t FUZZ_LENGTH = 4096; +volatile uint32_t FUZZ_POINTER = 0; +// Read the Byte of Input, if the Input is exausted trigger the breakpoint instead +static unsigned char fuzz_input_next(void) { + // printf("Get next Input from %lx \n",FUZZ_INPUT); + if (FUZZ_POINTER < FUZZ_LENGTH) { + FUZZ_POINTER++; + // printf("Input no. %d %x\n",FUZZ_POINTER-1,FUZZ_INPUT[FUZZ_POINTER-1]); + return FUZZ_INPUT[FUZZ_POINTER-1]; + } else { + // puts("End of Input"); + // Exausted inputs early + trigger_Qemu_break(); + } +} + +void my_lift_controller() +{ + lift_ctrl_get_vals(); + lift_ctrl_loop(); + lift_ctrl_set_vals(); +} + +// End Input Stuff +static void prvMover( void * pvParameters ); +static void prvController( void * pvParameters ); + +// Priorities +#define mainMOVER_TASK_PRIO ( tskIDLE_PRIORITY + 1 ) +#define mainCONTROLLER_TASK_PRIO ( tskIDLE_PRIORITY + 1 ) + +// Handles for direct messages +static TaskHandle_t xMover = NULL; +static TaskHandle_t xController = NULL; + +void main_lift( void ) +{ + /* Start the two tasks as described in the comments at the top of this + * file. */ +xTaskCreate( prvMover, /* The function that implements the task. */ + "Mover", /* The text name assigned to the task - for debug only as it is not used by the kernel. */ + configMINIMAL_STACK_SIZE, /* The size of the stack to allocate to the task. */ + NULL, /* The parameter passed to the task - not used in this case. */ + mainMOVER_TASK_PRIO, /* The priority assigned to the task. */ + &xMover ); /* The task handle is not required, so NULL is passed. */ + + xTaskCreate( prvController, /* The function that implements the task. */ + "Controller", /* The text name assigned to the task - for debug only as it is not used by the kernel. */ + configMINIMAL_STACK_SIZE, /* The size of the stack to allocate to the task. */ + NULL, /* The parameter passed to the task - not used in this case. */ + mainCONTROLLER_TASK_PRIO, /* The priority assigned to the task. */ + &xController ); /* The task handle is not required, so NULL is passed. */ + + /* Start the tasks and timer running. */ + vTaskStartScheduler(); + + /* If all is well, the scheduler will now be running, and the following + * line will never be reached. If the following line does execute, then + * there was insufficient FreeRTOS heap memory available for the Idle and/or + * timer tasks to be created. See the memory management section on the + * FreeRTOS web site for more details on the FreeRTOS heap + * http://www.freertos.org/a00111.html. */ + for( ; ; ) + { + } +} + +static void prvMover( void * pvParameters ) +{ + unsigned char buttons = 0; + while (1) + { + ulTaskNotifyTake(pdTRUE,portMAX_DELAY); + // generate new input + // take input as button inputs and generate a plausible sensor array + int my_lift_ctrl_io_in[ 10 ] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + // simulate top, load and bottom by inspecting lift_cnt + my_lift_ctrl_io_in[lift_SENS_TOP] = 864-10 <= lift_cnt; + my_lift_ctrl_io_in[lift_SENS_BOTTOM] = lift_cnt <= 0+10 ; + // assume load position is 346 = level 6 + my_lift_ctrl_io_in[lift_SENS_LOAD] = 336 <= lift_cnt && lift_cnt <= 356; + // assume an impulse if motor should run + my_lift_ctrl_io_in[lift_SENS_IMPULS] = lift_ctrl_io_out[lift_MOTOR_ON]; + // if (!lift_ctrl_io_out[lift_MOTOR_ON]) { + // whenever the motor stops we fetch a new button input + buttons = fuzz_input_next() & 0b00011111; // grab inputs for the 5 buttons + // } + // lift_GO_UP = 4, + // lift_GO_DOWN = 5 + // lift_GO_TOP = 6, + // lift_GO_BOTTOM = 7, + // lift_GO_LOAD = 8, + + // get_values reads the lowest bit into the lowest field + int sensors_in = 0, i; + _Pragma( "loopbound min 4 max 4" ) + for ( i = 3 ; i >= 0; --i ) { + sensors_in <<= 1; + sensors_in |= my_lift_ctrl_io_in[ i ] ? 1 : 0; + } + // set the new sensor input here, the notification that follows is just to keep the state + lift_simio_in = sensors_in<<5 | buttons; + + // send new sensor inputs + xTaskNotify(xController, sensors_in | lift_level<<4, eSetValueWithOverwrite); + } +} + +static void prvController( void * pvParameters ) +{ + lift_init(); + while (1) + { + xTaskNotifyGive(xMover); + lift_controller(); + ulTaskNotifyTake(pdTRUE,portMAX_DELAY); + } +} + +// void isr_starter( void ) +// { +// } + +/*-----------------------------------------------------------*/ diff --git a/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/tacle/app/lift/README b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/tacle/app/lift/README new file mode 100755 index 00000000..c7ada83a --- /dev/null +++ b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/tacle/app/lift/README @@ -0,0 +1,31 @@ +Lift Controller +=============== + +This is a port of Martin Schoeberl's Lift Controller [1] to C. The orignal +version is an embedded Java program that controls a lift in a factory in Turkey. +Benedikt Huber has ported the controller to C and Lucid [3]. It has been used in +various research projects, including T-CREST[3] and the F-Shell test generator +[4]. + +Overview +-------- + +The sensors of the system consist of several buttons, a load sensor (to detect +if the lift is positioned at the load), sensors to detect bottom and top +position, and an index sensor for the motor. The motor is controlled using two +digital outputs, one for its direction and one to turn it off or on. + +The controller itself consists of an initialization routine ctrl_init(), the +controller step ctrl_loop(), which should be invoked every 10ms. Outside a test +environment, ctrl_get_vals() should be called to read sensors before the +controller runs, and ctrl_set_vals() should be called to set actuators after the +controller ran. + +The digital IO interface (9 digital inputs, 2 digital outputs and 14 LEDs are +used) is realized as global volatile variables (simio_in, simio_out, simio_led), +that can be modified and read by test drivers or by the runtime environment. + +[1] http://www.jopdesign.com/doc/jembench.pdf +[2] https://github.com/visq/99lifts +[3] http://www.t-crest.org +[4] http://forsyte.at/software/fshell/ diff --git a/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/tacle/app/lift/changeLog.txt b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/tacle/app/lift/changeLog.txt new file mode 100755 index 00000000..babf8fb7 --- /dev/null +++ b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/tacle/app/lift/changeLog.txt @@ -0,0 +1,46 @@ +File: duff.c +Original provenience: Lift Controller, see README file + + +2015-12-21: +- replaced defines, introduced 4 enums to replace #define statements + (enum Direction, Sensor, Motor, Command) +- renamed libs to fit general naming scheme: lift_io -> liftio +- benchmark consisted of two benchmarks, test_lift and run_lift, + only run_lift remains, i.e., test_lift.data.c/h and test_lift.c removed. +- Makefile removed, file can now be compiled using 'gcc lift.c' +- 'uint16_t' replaced by 'unsigned short int' to get rid of 'stdint.h; +- changed LEVEL_POS_LENGTH from 14 to 16 (16 values were initialized). +- added an additional global variable 'int checksum' +- added function pre-fix 'lift_' to each function +- added function int lift_return() returning the checksum and lift_init() +- global variable checksum filled in function 'lift_ctrl_set_vals()' + and initialized to 0 in 'lift_init()' +- Added forward declarations of all functions before the declarations of global + variables +- in function lift_main, explicit initialization 'int i = 0' added. +- Re-ordered functions to fit template-order +- Applied code formatting according to the following rules + (incomplete, to be discussed; I basically used astyle with the attached + options file): + - Lines shall not be wider than 80 characters; whenever possible, appropriate + line breaks shall be inserted to keep lines below 80 characters + - Indentation is done using whitespaces only, no tabs. Code is indented by + two whitespaces + - Two empty lines are put between any two functions + - In non-empty lists or index expressions, opening '(' and '[' are followed by + one whitespace, closing ')' and ']' are preceded by one whitespace + - In comma- or colon-separated argument lists, one whitespace is put after + each comma/colon + - Names of functions and global variables all start with a benchmark-specific + prefix (here: st_) followed by lowercase letter (e.g., st_square) + - For pointer types, one whitespace is put before the '*' + - Operators within expressions shall be preceded and followed by one + whitespace + - Code of then- and else-parts of if-then-else statements shall be put in + separate lines, not in the same lines as the if-condition or the keyword + "else" + - Opening braces '{' denoting the beginning of code for some if-else or loop + body shall be put at the end of the same line where the keywords "if", + "else", "for", "while" etc. occur +- Added general TACLeBench header to beginning of source code diff --git a/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/tacle/app/lift/lift.c b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/tacle/app/lift/lift.c new file mode 100755 index 00000000..a79afa57 --- /dev/null +++ b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/tacle/app/lift/lift.c @@ -0,0 +1,132 @@ +/* + + This program is part of the TACLeBench benchmark suite. + Version V 2.0 + + Name: lift + + Author: Martin Schoeberl, Benedikt Huber + + Function: Lift Controler + + Source: C-Port from http://www.jopdesign.com/doc/jembench.pdf + + Original name: run_lift.c + + Changes: no major functional changes + + License: GPL version 3 or later + +*/ + + +/* + Include section +*/ + +#include "liftlibio.h" +#include "liftlibcontrol.h" + + +/* + Forward declaration of functions +*/ + +void lift_controller(); +void lift_init(); +void lift_main(); +int lift_return(); + + +/* + Declaration of global variables +*/ + +int lift_checksum;/* Checksum */ + + +/* + Initialization- and return-value-related functions +*/ + +void lift_init() +{ + unsigned int i; + unsigned char *p; + volatile char bitmask = 0; + + /* + Apply volatile XOR-bitmask to entire input array. + */ + p = ( unsigned char * ) &lift_ctrl_io_in[ 0 ]; + _Pragma( "loopbound min 40 max 40" ) + for ( i = 0; i < sizeof( lift_ctrl_io_in ); ++i, ++p ) + *p ^= bitmask; + + p = ( unsigned char * ) &lift_ctrl_io_out[ 0 ]; + _Pragma( "loopbound min 16 max 16" ) + for ( i = 0; i < sizeof( lift_ctrl_io_out ); ++i, ++p ) + *p ^= bitmask; + + p = ( unsigned char * ) &lift_ctrl_io_analog[ 0 ]; + _Pragma( "loopbound min 16 max 16" ) + for ( i = 0; i < sizeof( lift_ctrl_io_analog ); ++i, ++p ) + *p ^= bitmask; + + p = ( unsigned char * ) &lift_ctrl_io_led[ 0 ]; + _Pragma( "loopbound min 64 max 64" ) + for ( i = 0; i < sizeof( lift_ctrl_io_led ); ++i, ++p ) + *p ^= bitmask; + + lift_checksum = 0; + lift_ctrl_init(); +} + + +__attribute__((noinline)) int lift_return() +{ + return ( lift_checksum - 4005888 != 0 ); +} + + +/* + Algorithm core functions +*/ + +void lift_controller() +{ + lift_ctrl_get_vals(); + lift_ctrl_loop(); + lift_ctrl_set_vals(); +} + + +/* + Main functions +*/ + +void _Pragma( "entrypoint" ) lift_main() +{ + int i = 0; + _Pragma( "loopbound min 1001 max 1001" ) + while ( 1 ) { + /* zero input stimulus */ + lift_simio_in = 0; + lift_simio_adc1 = 0; + lift_simio_adc2 = 0; + lift_simio_adc3 = 0; + /* run lift_controller */ + lift_controller(); + if ( i++ >= 1000 ) + break; + } +} + + +// int main( void ) +// { +// lift_init(); +// lift_main(); + +// return ( lift_return() ); +// } diff --git a/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/tacle/app/lift/liftlibcontrol.c b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/tacle/app/lift/liftlibcontrol.c new file mode 100755 index 00000000..f8181817 --- /dev/null +++ b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/tacle/app/lift/liftlibcontrol.c @@ -0,0 +1,284 @@ +#include "liftlibio.h" +#include "liftlibcontrol.h" + +/* Global variables */ +int lift_levelPos[ 16 ]; // ? Sensed positions corresponding to 16 levels +int lift_one_level; + +/** + Is the counter valid for level positioning? +*/ +int lift_cntValid; + +/** + Position absolute or relative<. +*/ +int lift_cnt; + +/** + Last stoped level (1..13) if position is absolute else 0. +*/ +int lift_level; + +/** + load position in level, 0 means we don't know +*/ +int lift_loadLevel; + +/** + we're going TOP or BOTTOM, but stop at load position. +*/ +int lift_loadPending; + +/** + we're waiting for the load sensor to go. +*/ +int lift_loadSensor; + +/** + cmd keeps the value of the command until the command is finished. + It is only updated by the switches if it's current value is CMD_NONE. +*/ +int lift_cmd; + +// ? The time the motor will run for +int lift_timMotor; + +// ? The time the impulse will be carried after the motor shuts off +int lift_timImp; + +/** + Remember last direction for impuls count after motor off; +*/ +int lift_directionUp; + +/** + Last value of impuls sensor. +*/ +int lift_lastImp; + +int lift_dbgCnt; + +/** + stop value for the counter. +*/ +int lift_endCnt; + + +void lift_ctrl_init() +{ + int i; + lift_checksum = 0; + + lift_io_init(); + lift_cntValid = 0; + lift_cnt = 0; + lift_cmd = lift_CMD_NONE; + lift_timMotor = 0; + lift_timImp = 0; + lift_directionUp = 1; + lift_lastImp = 0; + lift_loadLevel = 0; + lift_loadPending = 0; + lift_loadSensor = 0; + i = 0; + lift_levelPos[ i++ ] = 0; + lift_levelPos[ i++ ] = 58; // + 58 + lift_levelPos[ i++ ] = 115; // + 57 + lift_levelPos[ i++ ] = 173; // + 58 + lift_levelPos[ i++ ] = 230; // + 57 + lift_levelPos[ i++ ] = 288; // + 58 + lift_levelPos[ i++ ] = 346; + lift_levelPos[ i++ ] = 403; + lift_levelPos[ i++ ] = 461; + lift_levelPos[ i++ ] = 518; + lift_levelPos[ i++ ] = 576; + lift_levelPos[ i++ ] = 634; + lift_levelPos[ i++ ] = 691; + lift_levelPos[ i++ ] = 749; + lift_levelPos[ i++ ] = 806; + lift_levelPos[ i++ ] = 864; + lift_one_level = lift_levelPos[ 1 ]; // ? One level means ~58 units +} + +void lift_ctrl_loop() +{ + if ( lift_cmd == lift_CMD_NONE ) + lift_check_cmd(); + else { + lift_do_impulse( lift_ctrl_io_in[ lift_SENS_IMPULS ], + lift_ctrl_io_out[ lift_MOTOR_ON ], + lift_ctrl_io_in[ lift_SENS_BOTTOM ] ); + lift_do_cmd(); + } + lift_check_level(); + lift_ctrl_io_led[ 13 ] = ( lift_dbgCnt & 0x80 ) != 0; + ++lift_dbgCnt; +} + + +// ? set lift_level and the LED +void lift_check_level() +{ + int i; + int middle = lift_one_level >> 2; // ? 14 Units, 1/4 of a level, is this a bug? + if ( lift_cntValid ) { + _Pragma( "loopbound min 14 max 14" ) + for ( lift_level = 1; lift_level < 14; ++lift_level ) { + if ( lift_cnt < lift_levelPos[ lift_level ] - middle ) // ? find out around which level the lift is (variable offset by +1) + break; + } + } else + lift_level = 0; + _Pragma( "loopbound min 14 max 14" ) + for ( i = 0; i < 14; ++i ) + lift_ctrl_io_led[ i ] = ( i == lift_level - 1 ); // ? Set LED for levels +} + + +// ? sets up lisft_cmd accoring to the buttons and top+bottom sensors. also sets lift_timMotor=50 +void lift_check_cmd() +{ + if ( lift_loadPending ) { + if ( lift_ctrl_io_in[ lift_SENS_BOTTOM ] ) + lift_cmd = lift_CMD_TOP; + } else + if ( lift_ctrl_io_in[ lift_GO_UP ] ) { + if ( !lift_ctrl_io_in[ lift_SENS_TOP ] && lift_level != 14 ) + lift_cmd = lift_CMD_UP; + } else + if ( lift_ctrl_io_in[ lift_GO_DOWN ] ) { + if ( !lift_ctrl_io_in[ lift_SENS_BOTTOM ] && lift_level != 1 ) + lift_cmd = lift_CMD_DOWN; + } else + if ( lift_ctrl_io_in[ lift_GO_LOAD ] ) { + if ( lift_loadLevel != 0 && lift_level < lift_loadLevel ) + lift_cmd = lift_CMD_TOP; + else + lift_cmd = lift_CMD_BOTTOM; + lift_loadPending = 1; + lift_loadSensor = 0; + } else + if ( lift_ctrl_io_in[ lift_GO_TOP ] ) { + if ( !lift_ctrl_io_in[ lift_SENS_TOP ] ) + lift_cmd = lift_CMD_TOP; + } else + if ( lift_ctrl_io_in[ lift_GO_BOTTOM ] ) { + if ( !lift_ctrl_io_in[ lift_SENS_BOTTOM ] ) + lift_cmd = lift_CMD_BOTTOM; + } + if ( lift_cmd != lift_CMD_NONE ) + lift_timMotor = 50; +} + + +// ? applies impulse val, sets lift_cnt +void lift_do_impulse( int val, int motor, int reset ) +{ + if ( val && !lift_lastImp ) { // ? is current impulse set and previous one not + if ( motor || lift_timImp > 0 ) { + if ( lift_directionUp ) + ++lift_cnt; + else + --lift_cnt; + } + } + if ( reset ) { + lift_cnt = 0; + lift_cntValid = 1; + } + lift_lastImp = val; + if ( lift_timImp > 0 ) { // ? decrement impulse, once it runs out reset cmd. + --lift_timImp; + if ( lift_timImp == 0 && lift_cmd != lift_CMD_NONE ) + lift_cmd = lift_CMD_NONE; + } +} + + +// do nothing until lift_timMotor is 0, then maybe turn of the motor and reset lift_cmd. +void lift_do_cmd() +{ + int run = 0; + if ( lift_timMotor > 0 ) + lift_wait_for_motor_start(); + else { + run = lift_check_run(); + if ( lift_ctrl_io_out[ lift_MOTOR_ON ] && !run ) { + /* motor stopped: */ + lift_cmd = 99; + lift_timImp = 50; // ? once motor is stopped set impulse + } + lift_ctrl_io_out[ lift_MOTOR_ON ] = run; + } +} + + +// ? Sets lift_endCnt and sets MOTOR_UP output +void lift_wait_for_motor_start() +{ + int newLevel = 0; + --lift_timMotor; + lift_directionUp = ( lift_cmd == lift_CMD_UP || lift_cmd == lift_CMD_TOP ); + lift_ctrl_io_out[ lift_MOTOR_UP ] = lift_directionUp; + if ( !lift_cntValid ) { + lift_cnt = 0; /* use relative counter */ + if ( lift_cmd == lift_CMD_UP ) + lift_endCnt = lift_one_level; + else + lift_endCnt = -lift_one_level; + } else { + lift_endCnt = lift_cnt; + newLevel = -99; + if ( lift_cmd == lift_CMD_UP ) + newLevel = lift_level + 1; + else + if ( lift_cmd == lift_CMD_DOWN ) + newLevel = lift_level - 1; + --newLevel; /* lift_level is one based */ + if ( newLevel >= 0 && newLevel < 14 ) + lift_endCnt = lift_levelPos[ newLevel ]; + } +} + + +int lift_check_run() // ? Motor needs to run, based on CMD lift_cnt +{ + if ( lift_cmd == lift_CMD_UP ) { + if ( lift_cnt < lift_endCnt - 1 && !lift_ctrl_io_in[ lift_SENS_TOP ] ) + return 1; + } else + if ( lift_cmd == lift_CMD_DOWN ) { + if ( lift_cnt > lift_endCnt + 1 && !lift_ctrl_io_in[ lift_SENS_BOTTOM ] ) + return 1; + } else + if ( lift_cmd == lift_CMD_TOP ) { + if ( lift_loadPending && lift_ctrl_io_in[ lift_SENS_LOAD ] ) { + /* we are at lift_load position */ + lift_loadLevel = lift_level; + lift_loadPending = 0; + return 0; + } + if ( !lift_ctrl_io_in[ lift_SENS_TOP ] ) + return 1; + /* safe fallback if lift_load sensor does not work */ + lift_loadPending = 0; + } else + if ( lift_cmd == lift_CMD_BOTTOM ) { + if ( lift_loadPending ) { + if ( lift_loadSensor ) { + if ( !lift_ctrl_io_in[ lift_SENS_LOAD ] ) { + lift_loadSensor = 0; + /* we are at lift_load position */ + lift_loadPending = 0; + lift_loadLevel = lift_level; + return 0; + } + } + lift_loadSensor = lift_ctrl_io_in[ lift_SENS_LOAD ]; + } + if ( !lift_ctrl_io_in[ lift_SENS_BOTTOM ] ) + return 1; + } + return 0; +} diff --git a/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/tacle/app/lift/liftlibcontrol.h b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/tacle/app/lift/liftlibcontrol.h new file mode 100755 index 00000000..e1955ddc --- /dev/null +++ b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/tacle/app/lift/liftlibcontrol.h @@ -0,0 +1,64 @@ +#ifndef LIFTLIBCONTROL_H +#define LIFTLIBCONTROL_H + +enum lift_Direction { + lift_GO_LOAD = 8, + lift_GO_TOP = 6, + lift_GO_BOTTOM = 7, + lift_GO_UP = 4, + lift_GO_DOWN = 5 +}; + +enum lift_Sensor { + lift_SENS_IMPULS = 0, + lift_SENS_TOP = 1, + lift_SENS_BOTTOM = 2, + lift_SENS_LOAD = 3 +}; + +enum lift_Motor { + lift_MOTOR_ON = 0, + lift_MOTOR_UP = 1 +}; + +enum lift_Command { + lift_CMD_NONE = 0, + lift_CMD_TOP = 1, + lift_CMD_BOTTOM = 2, + lift_CMD_UP = 3, + lift_CMD_DOWN = 4 +}; + +/* Global variables */ +extern int lift_levelPos[ 16 ]; +extern int lift_one_level; +extern int lift_cntValid; +extern int lift_cnt; +extern int lift_level; +extern int lift_loadLevel; +extern int lift_loadPending; +extern int lift_loadSensor; +extern int lift_cmd; +extern int lift_timMotor; +extern int lift_timImp; +extern int lift_directionUp; +extern int lift_lastImp; +extern int lift_dbgCnt; +extern int lift_endCnt; + +/* Checksum */ +extern int lift_checksum; + +/* prototypes */ +void lift_ctrl_init(); +void lift_ctrl_loop(); + +/* internal prototypes */ +int lift_check_run(); +void lift_wait_for_motor_start(); +void lift_do_cmd(); +void lift_do_impulse( int val, int motor, int reset ); +void lift_check_cmd(); +void lift_check_level(); + +#endif diff --git a/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/tacle/app/lift/liftlibio.c b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/tacle/app/lift/liftlibio.c new file mode 100755 index 00000000..7680b1fe --- /dev/null +++ b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/tacle/app/lift/liftlibio.c @@ -0,0 +1,67 @@ +#include "liftlibio.h" + +/* Global variables */ +int lift_ctrl_io_in[ 10 ] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; +int lift_ctrl_io_out[ 4 ] = {0, 0, 0, 0}; +int lift_ctrl_io_analog[ 4 ] = {0, 0, 0, 0}; +int lift_ctrl_io_led[ 16 ] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; +int lift_ctrl_dly1; +int lift_ctrl_dly2; + +/* Simulated hardware */ +volatile short int lift_simio_in; +volatile short int lift_simio_out; +volatile short int lift_simio_led; +volatile short int lift_simio_adc1; +volatile short int lift_simio_adc2; +volatile short int lift_simio_adc3; + +void lift_io_init() +{ + lift_ctrl_dly1 = 0; + lift_ctrl_dly2 = 0; +} + + +// ? reads lift_ctrl_io_out and writes it into lift_simio_out +void lift_ctrl_set_vals() +{ + int val = 0, i; + _Pragma( "loopbound min 4 max 4" ) + for ( i = 4 - 1; i >= 0; --i ) { + val <<= 1; + val |= lift_ctrl_io_out[ i ] ? 1 : 0; + } + lift_simio_out = val; + _Pragma( "loopbound min 14 max 14" ) + for ( i = 14 - 1; i >= 0; --i ) { + val <<= 1; + val |= lift_ctrl_io_led[ i ] ? 1 : 0; + } + lift_simio_led = val; + lift_checksum += val; +} + + +// ? reads lift_simio_in and filters it though a majority voter +void lift_ctrl_get_vals() +{ + int i; + unsigned short int in0 = lift_simio_in; + unsigned short int in1 = lift_ctrl_dly1; + unsigned short int in2 = lift_ctrl_dly2; + lift_ctrl_dly2 = lift_ctrl_dly1; + lift_ctrl_dly1 = in0; + /* majority voting for input values + delays input value change by one period */ + _Pragma( "loopbound min 10 max 10" ) + for ( i = 0; i < 10; ++i ) { + lift_ctrl_io_in[ i ] = ( ( in0 & 1 ) + ( in1 & 1 ) + ( in2 & 1 ) ) > 1; + in0 >>= 1; + in1 >>= 1; + in2 >>= 1; + } + lift_ctrl_io_analog[ 0 ] = lift_simio_adc1; + lift_ctrl_io_analog[ 1 ] = lift_simio_adc2; + lift_ctrl_io_analog[ 2 ] = lift_simio_adc3; +} diff --git a/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/tacle/app/lift/liftlibio.h b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/tacle/app/lift/liftlibio.h new file mode 100755 index 00000000..088fe633 --- /dev/null +++ b/FreeRTOS/Demo/CORTEX_M3_MPS2_QEMU_GCC/tacle/app/lift/liftlibio.h @@ -0,0 +1,27 @@ +#ifndef LIFTLIBIO_H +#define LIFTLIBIO_H + +/* Global variables */ +extern int lift_ctrl_io_in[ 10 ]; +extern int lift_ctrl_io_out[ 4 ]; +extern int lift_ctrl_io_analog[ 4 ]; +extern int lift_ctrl_io_led[ 16 ]; +extern int lift_ctrl_dly1, lift_ctrl_dly2; + +/* Simulated hardware */ +extern volatile short int lift_simio_in; +extern volatile short int lift_simio_out; +extern volatile short int lift_simio_led; +extern volatile short int lift_simio_adc1; +extern volatile short int lift_simio_adc2; +extern volatile short int lift_simio_adc3; + +/* Checksum */ +extern int lift_checksum; + +/* prototypes */ +void lift_io_init( void ); +void lift_ctrl_get_vals( void ); +void lift_ctrl_set_vals( void ); + +#endif