From f3ffb73b62621464fee8cbbb4ff47c8697893a58 Mon Sep 17 00:00:00 2001 From: gab-k Date: Fri, 5 Jan 2024 00:37:21 +0100 Subject: [PATCH] Copy code from flash to ram in order to not use XIP allowing writing to flash memory while both cores are running; Motor control now operates with two states/modes; Extended Default CV Range to 1024 - refers to NMRA/RCN standards --- CMakeLists.txt | 5 +- CV.h | 522 ++++++++++++++++++++++++++++++++++++++++++++++++- core0.c | 6 +- core1.c | 298 ++++++++++++++-------------- core1.h | 80 ++++---- shared.c | 26 +-- 6 files changed, 738 insertions(+), 199 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 50d798f..198ee5a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -52,5 +52,8 @@ pico_enable_stdio_usb(RP2040-Decoder 0) # disable uart output (to enable put a 1 instead of the 0) pico_enable_stdio_uart(RP2040-Decoder 0) +# Copy code to from flash to memory, then run from memory +pico_set_binary_type(RP2040-Decoder copy_to_ram) + # create map/bin/hex file etc. -#pico_add_extra_outputs(RP2040-Decoder) +pico_add_extra_outputs(RP2040-Decoder) diff --git a/CV.h b/CV.h index 990db3f..c542775 100644 --- a/CV.h +++ b/CV.h @@ -1,9 +1,12 @@ #pragma once -#define CV_ARRAY_SIZE 512 +#include "pico/stdlib.h" + +#define CV_ARRAY_SIZE 1024 // All CVs are 8-bit numbers ranging from (0 - 255)dec = (0b00000000 - 0b11111111)bin = (0x00 - 0xFF)hex // Note : CV_1 = index 0, CV_2 = index 1, CV_3 = index 2, ... -uint8_t CV_ARRAY_DEFAULT [CV_ARRAY_SIZE] = { +// Explicitly write CV_ARRAY_DEFAULT with into flash memory using __in_flash attribute macro +uint8_t __in_flash("Default_CV_values") CV_ARRAY_DEFAULT [CV_ARRAY_SIZE] = { 0b00000011, //CV_1 - Basic address - CV_1 = 0 and CV_1 > 127 are not valid - Default = 3 0b00001000, //CV_2 - V_min - Default = 8 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -614,4 +617,519 @@ uint8_t CV_ARRAY_DEFAULT [CV_ARRAY_SIZE] = { 0b00000000, //CV_510 - byte 2 0b00000000, //CV_511 - byte 1 0b00000000, //CV_512 - byte 0 +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + 0b00000000, //CV_513 - + 0b00000000, //CV_514 - + 0b00000000, //CV_515 - + 0b00000000, //CV_516 - + 0b00000000, //CV_517 - + 0b00000000, //CV_518 - + 0b00000000, //CV_519 - + 0b00000000, //CV_520 - + 0b00000000, //CV_521 - + 0b00000000, //CV_522 - + 0b00000000, //CV_523 - + 0b00000000, //CV_524 - + 0b00000000, //CV_525 - + 0b00000000, //CV_526 - + 0b00000000, //CV_527 - + 0b00000000, //CV_528 - + 0b00000000, //CV_529 - + 0b00000000, //CV_530 - + 0b00000000, //CV_531 - + 0b00000000, //CV_532 - + 0b00000000, //CV_533 - + 0b00000000, //CV_534 - + 0b00000000, //CV_535 - + 0b00000000, //CV_536 - + 0b00000000, //CV_537 - + 0b00000000, //CV_538 - + 0b00000000, //CV_539 - + 0b00000000, //CV_540 - + 0b00000000, //CV_541 - + 0b00000000, //CV_542 - + 0b00000000, //CV_543 - + 0b00000000, //CV_544 - + 0b00000000, //CV_545 - + 0b00000000, //CV_546 - + 0b00000000, //CV_547 - + 0b00000000, //CV_548 - + 0b00000000, //CV_549 - + 0b00000000, //CV_550 - + 0b00000000, //CV_551 - + 0b00000000, //CV_552 - + 0b00000000, //CV_553 - + 0b00000000, //CV_554 - + 0b00000000, //CV_555 - + 0b00000000, //CV_556 - + 0b00000000, //CV_557 - + 0b00000000, //CV_558 - + 0b00000000, //CV_559 - + 0b00000000, //CV_560 - + 0b00000000, //CV_561 - + 0b00000000, //CV_562 - + 0b00000000, //CV_563 - + 0b00000000, //CV_564 - + 0b00000000, //CV_565 - + 0b00000000, //CV_566 - + 0b00000000, //CV_567 - + 0b00000000, //CV_568 - + 0b00000000, //CV_569 - + 0b00000000, //CV_570 - + 0b00000000, //CV_571 - + 0b00000000, //CV_572 - + 0b00000000, //CV_573 - + 0b00000000, //CV_574 - + 0b00000000, //CV_575 - + 0b00000000, //CV_576 - + 0b00000000, //CV_577 - + 0b00000000, //CV_578 - + 0b00000000, //CV_579 - + 0b00000000, //CV_580 - + 0b00000000, //CV_581 - + 0b00000000, //CV_582 - + 0b00000000, //CV_583 - + 0b00000000, //CV_584 - + 0b00000000, //CV_585 - + 0b00000000, //CV_586 - + 0b00000000, //CV_587 - + 0b00000000, //CV_588 - + 0b00000000, //CV_589 - + 0b00000000, //CV_590 - + 0b00000000, //CV_591 - + 0b00000000, //CV_592 - + 0b00000000, //CV_593 - + 0b00000000, //CV_594 - + 0b00000000, //CV_595 - + 0b00000000, //CV_596 - + 0b00000000, //CV_597 - + 0b00000000, //CV_598 - + 0b00000000, //CV_599 - + 0b00000000, //CV_600 - + 0b00000000, //CV_601 - + 0b00000000, //CV_602 - + 0b00000000, //CV_603 - + 0b00000000, //CV_604 - + 0b00000000, //CV_605 - + 0b00000000, //CV_606 - + 0b00000000, //CV_607 - + 0b00000000, //CV_608 - + 0b00000000, //CV_609 - + 0b00000000, //CV_610 - + 0b00000000, //CV_611 - + 0b00000000, //CV_612 - + 0b00000000, //CV_613 - + 0b00000000, //CV_614 - + 0b00000000, //CV_615 - + 0b00000000, //CV_616 - + 0b00000000, //CV_617 - + 0b00000000, //CV_618 - + 0b00000000, //CV_619 - + 0b00000000, //CV_620 - + 0b00000000, //CV_621 - + 0b00000000, //CV_622 - + 0b00000000, //CV_623 - + 0b00000000, //CV_624 - + 0b00000000, //CV_625 - + 0b00000000, //CV_626 - + 0b00000000, //CV_627 - + 0b00000000, //CV_628 - + 0b00000000, //CV_629 - + 0b00000000, //CV_630 - + 0b00000000, //CV_631 - + 0b00000000, //CV_632 - + 0b00000000, //CV_633 - + 0b00000000, //CV_634 - + 0b00000000, //CV_635 - + 0b00000000, //CV_636 - + 0b00000000, //CV_637 - + 0b00000000, //CV_638 - + 0b00000000, //CV_639 - + 0b00000000, //CV_640 - + 0b00000000, //CV_641 - + 0b00000000, //CV_642 - + 0b00000000, //CV_643 - + 0b00000000, //CV_644 - + 0b00000000, //CV_645 - + 0b00000000, //CV_646 - + 0b00000000, //CV_647 - + 0b00000000, //CV_648 - + 0b00000000, //CV_649 - + 0b00000000, //CV_650 - + 0b00000000, //CV_651 - + 0b00000000, //CV_652 - + 0b00000000, //CV_653 - + 0b00000000, //CV_654 - + 0b00000000, //CV_655 - + 0b00000000, //CV_656 - + 0b00000000, //CV_657 - + 0b00000000, //CV_658 - + 0b00000000, //CV_659 - + 0b00000000, //CV_660 - + 0b00000000, //CV_661 - + 0b00000000, //CV_662 - + 0b00000000, //CV_663 - + 0b00000000, //CV_664 - + 0b00000000, //CV_665 - + 0b00000000, //CV_666 - + 0b00000000, //CV_667 - + 0b00000000, //CV_668 - + 0b00000000, //CV_669 - + 0b00000000, //CV_670 - + 0b00000000, //CV_671 - + 0b00000000, //CV_672 - + 0b00000000, //CV_673 - + 0b00000000, //CV_674 - + 0b00000000, //CV_675 - + 0b00000000, //CV_676 - + 0b00000000, //CV_677 - + 0b00000000, //CV_678 - + 0b00000000, //CV_679 - + 0b00000000, //CV_680 - + 0b00000000, //CV_681 - + 0b00000000, //CV_682 - + 0b00000000, //CV_683 - + 0b00000000, //CV_684 - + 0b00000000, //CV_685 - + 0b00000000, //CV_686 - + 0b00000000, //CV_687 - + 0b00000000, //CV_688 - + 0b00000000, //CV_689 - + 0b00000000, //CV_690 - + 0b00000000, //CV_691 - + 0b00000000, //CV_692 - + 0b00000000, //CV_693 - + 0b00000000, //CV_694 - + 0b00000000, //CV_695 - + 0b00000000, //CV_696 - + 0b00000000, //CV_697 - + 0b00000000, //CV_698 - + 0b00000000, //CV_699 - + 0b00000000, //CV_700 - + 0b00000000, //CV_701 - + 0b00000000, //CV_702 - + 0b00000000, //CV_703 - + 0b00000000, //CV_704 - + 0b00000000, //CV_705 - + 0b00000000, //CV_706 - + 0b00000000, //CV_707 - + 0b00000000, //CV_708 - + 0b00000000, //CV_709 - + 0b00000000, //CV_710 - + 0b00000000, //CV_711 - + 0b00000000, //CV_712 - + 0b00000000, //CV_713 - + 0b00000000, //CV_714 - + 0b00000000, //CV_715 - + 0b00000000, //CV_716 - + 0b00000000, //CV_717 - + 0b00000000, //CV_718 - + 0b00000000, //CV_719 - + 0b00000000, //CV_720 - + 0b00000000, //CV_721 - + 0b00000000, //CV_722 - + 0b00000000, //CV_723 - + 0b00000000, //CV_724 - + 0b00000000, //CV_725 - + 0b00000000, //CV_726 - + 0b00000000, //CV_727 - + 0b00000000, //CV_728 - + 0b00000000, //CV_729 - + 0b00000000, //CV_730 - + 0b00000000, //CV_731 - + 0b00000000, //CV_732 - + 0b00000000, //CV_733 - + 0b00000000, //CV_734 - + 0b00000000, //CV_735 - + 0b00000000, //CV_736 - + 0b00000000, //CV_737 - + 0b00000000, //CV_738 - + 0b00000000, //CV_739 - + 0b00000000, //CV_740 - + 0b00000000, //CV_741 - + 0b00000000, //CV_742 - + 0b00000000, //CV_743 - + 0b00000000, //CV_744 - + 0b00000000, //CV_745 - + 0b00000000, //CV_746 - + 0b00000000, //CV_747 - + 0b00000000, //CV_748 - + 0b00000000, //CV_749 - + 0b00000000, //CV_750 - + 0b00000000, //CV_751 - + 0b00000000, //CV_752 - + 0b00000000, //CV_753 - + 0b00000000, //CV_754 - + 0b00000000, //CV_755 - + 0b00000000, //CV_756 - + 0b00000000, //CV_757 - + 0b00000000, //CV_758 - + 0b00000000, //CV_759 - + 0b00000000, //CV_760 - + 0b00000000, //CV_761 - + 0b00000000, //CV_762 - + 0b00000000, //CV_763 - + 0b00000000, //CV_764 - + 0b00000000, //CV_765 - + 0b00000000, //CV_766 - + 0b00000000, //CV_767 - + 0b00000000, //CV_768 - + 0b00000000, //CV_769 - + 0b00000000, //CV_770 - + 0b00000000, //CV_771 - + 0b00000000, //CV_772 - + 0b00000000, //CV_773 - + 0b00000000, //CV_774 - + 0b00000000, //CV_775 - + 0b00000000, //CV_776 - + 0b00000000, //CV_777 - + 0b00000000, //CV_778 - + 0b00000000, //CV_779 - + 0b00000000, //CV_780 - + 0b00000000, //CV_781 - + 0b00000000, //CV_782 - + 0b00000000, //CV_783 - + 0b00000000, //CV_784 - + 0b00000000, //CV_785 - + 0b00000000, //CV_786 - + 0b00000000, //CV_787 - + 0b00000000, //CV_788 - + 0b00000000, //CV_789 - + 0b00000000, //CV_790 - + 0b00000000, //CV_791 - + 0b00000000, //CV_792 - + 0b00000000, //CV_793 - + 0b00000000, //CV_794 - + 0b00000000, //CV_795 - + 0b00000000, //CV_796 - + 0b00000000, //CV_797 - + 0b00000000, //CV_798 - + 0b00000000, //CV_799 - + 0b00000000, //CV_800 - + 0b00000000, //CV_801 - + 0b00000000, //CV_802 - + 0b00000000, //CV_803 - + 0b00000000, //CV_804 - + 0b00000000, //CV_805 - + 0b00000000, //CV_806 - + 0b00000000, //CV_807 - + 0b00000000, //CV_808 - + 0b00000000, //CV_809 - + 0b00000000, //CV_810 - + 0b00000000, //CV_811 - + 0b00000000, //CV_812 - + 0b00000000, //CV_813 - + 0b00000000, //CV_814 - + 0b00000000, //CV_815 - + 0b00000000, //CV_816 - + 0b00000000, //CV_817 - + 0b00000000, //CV_818 - + 0b00000000, //CV_819 - + 0b00000000, //CV_820 - + 0b00000000, //CV_821 - + 0b00000000, //CV_822 - + 0b00000000, //CV_823 - + 0b00000000, //CV_824 - + 0b00000000, //CV_825 - + 0b00000000, //CV_826 - + 0b00000000, //CV_827 - + 0b00000000, //CV_828 - + 0b00000000, //CV_829 - + 0b00000000, //CV_830 - + 0b00000000, //CV_831 - + 0b00000000, //CV_832 - + 0b00000000, //CV_833 - + 0b00000000, //CV_834 - + 0b00000000, //CV_835 - + 0b00000000, //CV_836 - + 0b00000000, //CV_837 - + 0b00000000, //CV_838 - + 0b00000000, //CV_839 - + 0b00000000, //CV_840 - + 0b00000000, //CV_841 - + 0b00000000, //CV_842 - + 0b00000000, //CV_843 - + 0b00000000, //CV_844 - + 0b00000000, //CV_845 - + 0b00000000, //CV_846 - + 0b00000000, //CV_847 - + 0b00000000, //CV_848 - + 0b00000000, //CV_849 - + 0b00000000, //CV_850 - + 0b00000000, //CV_851 - + 0b00000000, //CV_852 - + 0b00000000, //CV_853 - + 0b00000000, //CV_854 - + 0b00000000, //CV_855 - + 0b00000000, //CV_856 - + 0b00000000, //CV_857 - + 0b00000000, //CV_858 - + 0b00000000, //CV_859 - + 0b00000000, //CV_860 - + 0b00000000, //CV_861 - + 0b00000000, //CV_862 - + 0b00000000, //CV_863 - + 0b00000000, //CV_864 - + 0b00000000, //CV_865 - + 0b00000000, //CV_866 - + 0b00000000, //CV_867 - + 0b00000000, //CV_868 - + 0b00000000, //CV_869 - + 0b00000000, //CV_870 - + 0b00000000, //CV_871 - + 0b00000000, //CV_872 - + 0b00000000, //CV_873 - + 0b00000000, //CV_874 - + 0b00000000, //CV_875 - + 0b00000000, //CV_876 - + 0b00000000, //CV_877 - + 0b00000000, //CV_878 - + 0b00000000, //CV_879 - + 0b00000000, //CV_880 - + 0b00000000, //CV_881 - + 0b00000000, //CV_882 - + 0b00000000, //CV_883 - + 0b00000000, //CV_884 - + 0b00000000, //CV_885 - + 0b00000000, //CV_886 - + 0b00000000, //CV_887 - + 0b00000000, //CV_888 - + 0b00000000, //CV_889 - + 0b00000000, //CV_890 - + 0b00000000, //CV_891 - + 0b00000000, //CV_892 - + 0b00000000, //CV_893 - + 0b00000000, //CV_894 - + 0b00000000, //CV_895 - + 0b00000000, //CV_896 - + 0b00000000, //CV_897 - + 0b00000000, //CV_898 - + 0b00000000, //CV_899 - + 0b00000000, //CV_900 - + 0b00000000, //CV_901 - + 0b00000000, //CV_902 - + 0b00000000, //CV_903 - + 0b00000000, //CV_904 - + 0b00000000, //CV_905 - + 0b00000000, //CV_906 - + 0b00000000, //CV_907 - + 0b00000000, //CV_908 - + 0b00000000, //CV_909 - + 0b00000000, //CV_910 - + 0b00000000, //CV_911 - + 0b00000000, //CV_912 - + 0b00000000, //CV_913 - + 0b00000000, //CV_914 - + 0b00000000, //CV_915 - + 0b00000000, //CV_916 - + 0b00000000, //CV_917 - + 0b00000000, //CV_918 - + 0b00000000, //CV_919 - + 0b00000000, //CV_920 - + 0b00000000, //CV_921 - + 0b00000000, //CV_922 - + 0b00000000, //CV_923 - + 0b00000000, //CV_924 - + 0b00000000, //CV_925 - + 0b00000000, //CV_926 - + 0b00000000, //CV_927 - + 0b00000000, //CV_928 - + 0b00000000, //CV_929 - + 0b00000000, //CV_930 - + 0b00000000, //CV_931 - + 0b00000000, //CV_932 - + 0b00000000, //CV_933 - + 0b00000000, //CV_934 - + 0b00000000, //CV_935 - + 0b00000000, //CV_936 - + 0b00000000, //CV_937 - + 0b00000000, //CV_938 - + 0b00000000, //CV_939 - + 0b00000000, //CV_940 - + 0b00000000, //CV_941 - + 0b00000000, //CV_942 - + 0b00000000, //CV_943 - + 0b00000000, //CV_944 - + 0b00000000, //CV_945 - + 0b00000000, //CV_946 - + 0b00000000, //CV_947 - + 0b00000000, //CV_948 - + 0b00000000, //CV_949 - + 0b00000000, //CV_950 - + 0b00000000, //CV_951 - + 0b00000000, //CV_952 - + 0b00000000, //CV_953 - + 0b00000000, //CV_954 - + 0b00000000, //CV_955 - + 0b00000000, //CV_956 - + 0b00000000, //CV_957 - + 0b00000000, //CV_958 - + 0b00000000, //CV_959 - + 0b00000000, //CV_960 - + 0b00000000, //CV_961 - + 0b00000000, //CV_962 - + 0b00000000, //CV_963 - + 0b00000000, //CV_964 - + 0b00000000, //CV_965 - + 0b00000000, //CV_966 - + 0b00000000, //CV_967 - + 0b00000000, //CV_968 - + 0b00000000, //CV_969 - + 0b00000000, //CV_970 - + 0b00000000, //CV_971 - + 0b00000000, //CV_972 - + 0b00000000, //CV_973 - + 0b00000000, //CV_974 - + 0b00000000, //CV_975 - + 0b00000000, //CV_976 - + 0b00000000, //CV_977 - + 0b00000000, //CV_978 - + 0b00000000, //CV_979 - + 0b00000000, //CV_980 - + 0b00000000, //CV_981 - + 0b00000000, //CV_982 - + 0b00000000, //CV_983 - + 0b00000000, //CV_984 - + 0b00000000, //CV_985 - + 0b00000000, //CV_986 - + 0b00000000, //CV_987 - + 0b00000000, //CV_988 - + 0b00000000, //CV_989 - + 0b00000000, //CV_990 - + 0b00000000, //CV_991 - + 0b00000000, //CV_992 - + 0b00000000, //CV_993 - + 0b00000000, //CV_994 - + 0b00000000, //CV_995 - + 0b00000000, //CV_996 - + 0b00000000, //CV_997 - + 0b00000000, //CV_998 - + 0b00000000, //CV_999 - + 0b00000000, //CV_1000 - + 0b00000000, //CV_1001 - + 0b00000000, //CV_1002 - + 0b00000000, //CV_1003 - + 0b00000000, //CV_1004 - + 0b00000000, //CV_1005 - + 0b00000000, //CV_1006 - + 0b00000000, //CV_1007 - + 0b00000000, //CV_1008 - + 0b00000000, //CV_1009 - + 0b00000000, //CV_1010 - + 0b00000000, //CV_1011 - + 0b00000000, //CV_1012 - + 0b00000000, //CV_1013 - + 0b00000000, //CV_1014 - + 0b00000000, //CV_1015 - + 0b00000000, //CV_1016 - + 0b00000000, //CV_1017 - + 0b00000000, //CV_1018 - + 0b00000000, //CV_1019 - + 0b00000000, //CV_1020 - + 0b00000000, //CV_1021 - + 0b00000000, //CV_1022 - + 0b00000000, //CV_1023 - + 0b00000000, //CV_1024 - }; + + diff --git a/core0.c b/core0.c index b80e073..e5456ec 100644 --- a/core0.c +++ b/core0.c @@ -584,7 +584,7 @@ void cv_setup_check() { program_mode(4, arr); //reset to CV_ARRAY_DEFAULT (write CV_8 = 8) } - // Check for existing ADC offset setup + // Check for existing ADC offset setup TODO: always measure? if (CV_ARRAY_FLASH[171] == 0xFF) { // Write a value of 7 to read-only CV_7 in order to trigger a ADC offset measurement LOG(1, "found cv[171] equals to 0xff, adc offset adjstments (and cr[7] = 7)\n"); @@ -594,6 +594,7 @@ void cv_setup_check() { // Check for base PWM configuration - used for feed-forward // Forward Direction + /* if (get_16bit_CV(175) == 0) { LOG(1, "found cv[175] equals to 0, forward direction\n"); const uint16_t base_pwm_fwd = measure_base_pwm(true, 10); @@ -615,6 +616,7 @@ void cv_setup_check() { const uint8_t arr1[4] = {125, base_pwm_rev_low_byte, 178, 124}; program_mode(4, arr1); } + */ LOG(1, "int_lim_max %d\n", CV_ARRAY_FLASH[51]); LOG(1, "int_lim_min %d\n", CV_ARRAY_FLASH[52]); } @@ -666,5 +668,5 @@ int main() { LOG(1, "core0 done\n"); busy_wait_ms(100); multicore_launch_core1(core1_entry); - while (true); + while (true); //sleep_ms(100); //TODO: any potential issues when using sleep? } \ No newline at end of file diff --git a/core1.c b/core1.c index 276d4b6..b85b8f6 100644 --- a/core1.c +++ b/core1.c @@ -27,7 +27,7 @@ uint8_t get_speed_step_table_index_of_speed_step(uint8_t speed_step) { bool speed_helper(struct repeating_timer *const t) { // pid->setpoint only gets adjusted when the speed_helper_counter is equal to the accel_rate/decel_rate // -> Time for 1 Speed Step := (speed_helper timer delay)*(accel_rate) or CV_175*CV_3 or CV_175*CV_4 - pid_params *const pid = (pid_params *) (t->user_data); + controller_parameter_t *const pid = (controller_parameter_t *) (t->user_data); const uint8_t accel_rate = CV_ARRAY_FLASH[2]; const uint8_t decel_rate = CV_ARRAY_FLASH[3]; static uint8_t speed_helper_counter; @@ -38,7 +38,7 @@ bool speed_helper(struct repeating_timer *const t) { const bool direction_changed = get_direction_of_speed_step(speed_step_target) != get_direction_of_speed_step(speed_step_target_prev); - if (pid->speed_step_target == 129 || pid->speed_step_target == 1) { + if (speed_step_target == 129 || speed_step_target == 1) { // Emergency Stop speed_table_index = 0; speed_helper_counter = 0; @@ -77,7 +77,6 @@ bool speed_helper(struct repeating_timer *const t) { return true; } - // Helper function to adjust pwm level/duty cycle. void adjust_pwm_level(const uint16_t level) { if (get_direction_of_speed_step(speed_step_target)) { @@ -93,50 +92,8 @@ void adjust_pwm_level(const uint16_t level) { } -// Update feed-forward slope/gradient for both directions -void update_m(pid_params *const pid) { - pid->m_fwd = (pid->y_2_fwd - pid->y_1_fwd) / (pid->x_2_fwd - pid->x_1_fwd); - pid->m_rev = (pid->y_2_rev - pid->y_1_rev) / (pid->x_2_rev - pid->x_1_rev); -} - - -// Update feedforward function value y_2 in both directions -void update_y(pid_params *const pid, const float i) { - if (get_direction_of_speed_step(speed_step_target)) { - // Forward - pid->y_2_fwd += i * pid->k_ff; - // Limit y_2_fwd - if (pid->y_2_fwd > pid->max_output) pid->y_2_fwd = pid->max_output; - else if (pid->y_2_fwd < pid->y_1_fwd) pid->y_2_fwd = pid->y_1_fwd; - } - else { - // Reverse - pid->y_2_rev += i * pid->k_ff; - // Limit y_2_rev - if (pid->y_2_rev > pid->max_output) pid->y_2_rev = pid->max_output; - else if (pid->y_2_rev < pid->y_1_rev) pid->y_2_rev = pid->y_1_rev; - } - update_m(pid); -} - - -// Returns feedforward value corresponding to current setpoint -float get_ff_val(const pid_params *const pid) { - float val; - if (get_direction_of_speed_step(speed_step_target)) { - // Forward - val = pid->m_fwd * ((float) pid->setpoint - pid->x_1_fwd) + pid->y_1_fwd; - } - else { - // Reverse - val = pid->m_rev * ((float) pid->setpoint - pid->x_1_rev) + pid->y_1_rev; - } - return val; -} - - // Returns proportional gain value corresponding to current setpoint -float get_kp(const pid_params *const pid) { +float get_kp(const controller_parameter_t *const pid) { const float sp = (float) pid->setpoint; if (sp < pid->k_p_x_1) { return pid->k_p_m_1 * sp + pid->k_p_y_0; @@ -145,81 +102,133 @@ float get_kp(const pid_params *const pid) { } -// PID controller function gets called every x milliseconds where x is CV_49 aka sampling time (pid->t) -bool pid_control(struct repeating_timer *const t) { - // Cast (void *) to (pid_params *) - pid_params *const pid = (pid_params *) (t->user_data); - - // Acceleration from standstill or change in direction -> reset previous derivative, error and integral parts - const bool direction_changed = get_direction_of_speed_step(speed_step_target) != - get_direction_of_speed_step(speed_step_target_prev); - if ((!pid->setpoint_prev && !pid->setpoint) || direction_changed) { - pid->d_prev = 0.0f; - pid->i_prev = 0.0f; - pid->e_prev = 0.0f; +uint16_t get_initial_level(controller_parameter_t *const ctrl_par){ + uint32_t sum = 0; + uint32_t i = 0; + while (i < 16) { + if (ctrl_par->base_pwm_arr[i] > 0) { + sum += ctrl_par->base_pwm_arr[i]; + i++; + } + else { + break; + } } + return ((sum / i)*2)/3; +} - // Get feed-forward and proportional gain values - const float feed_fwd = get_ff_val(pid); - pid->k_p = get_kp(pid); - - // Measure BEMF voltage and compute error - pid->measurement = measure(pid->msr_total_iterations, - pid->msr_delay_in_us, - pid->l_side_arr_cutoff, - pid->r_side_arr_cutoff, - get_direction_of_speed_step(speed_step_target)); - - const float measurement_corrected = pid->measurement - pid->adc_offset; - const float e = (float) pid->setpoint - measurement_corrected; +// Controller - Startup mode +void controller_startup_mode(controller_parameter_t *const ctrl_par) { + if(ctrl_par->output == 0) { + ctrl_par->output = get_initial_level(ctrl_par); + } + if (ctrl_par->measurement_corrected < 7.5f){ + const uint16_t max_level = _125M / (CV_ARRAY_FLASH[8] * 100 + 10000); + adjust_pwm_level(ctrl_par->output); + ctrl_par->output += max_level / 250; + if (ctrl_par->output > max_level) { + // Try again with half val... + ctrl_par->output = get_initial_level(ctrl_par)/2; + } + } + else { + // Save level value in array + ctrl_par->base_pwm_arr[ctrl_par->base_pwm_arr_i] = ctrl_par->output; + // Update index + ctrl_par->base_pwm_arr_i = (ctrl_par->base_pwm_arr_i+1)%16; + // Set ramp_up_mode flag to true + ctrl_par->mode = PID_MODE; + // multiply with constant value of 0.9 + ctrl_par->feed_fwd = 0.9f * (float)ctrl_par->output; + } +} +// Controller - PID control mode +void controller_pid_mode(controller_parameter_t *const ctrl_par) { + ctrl_par->k_p = get_kp(ctrl_par); // Proportional part, integral part and derivative part (including digital low-pass-filter with time constant tau) - const float p = pid->k_p * e; - float i = pid->ci_0 * (e + pid->e_prev) + pid->i_prev; - const float d = pid->cd_0 * (pid->measurement - pid->measurement_prev) + pid->cd_1 * pid->d_prev; + float p = ctrl_par->k_p * ctrl_par->e; + float i = ctrl_par->ci_0 * (ctrl_par->e + ctrl_par->e_prev) + ctrl_par->i_prev; + float d = ctrl_par->cd_0 * (ctrl_par->measurement - ctrl_par->measurement_prev) + ctrl_par->cd_1 * ctrl_par->d_prev; // Check for limits on the integral part - if (i > pid->int_lim_max) { - i = pid->int_lim_max; + if (i > ctrl_par->int_lim_max) { + i = ctrl_par->int_lim_max; } - else if (i < pid->int_lim_min) { - i = pid->int_lim_min; + else if (i < ctrl_par->int_lim_min) { + i = ctrl_par->int_lim_min; } // Sum feed forward + all controller terms (p+i+d). Limit Output from 0% to 100% duty cycle - float output = feed_fwd + p + i + d; - if (output < 0 || !pid->setpoint) { - output = 0.0f; + float output_f = ctrl_par->feed_fwd + p + i + d; + if (output_f < 0) { + output_f = 0.0f; } - else if (output > pid->max_output) { - output = pid->max_output; + else if (output_f > ctrl_par->max_output) { + output_f = ctrl_par->max_output; } if (LOGLEVEL >= 1) { static int counter = 0; counter++; if (counter > 100) { - LOG(1, "pid error(%f) output(%f)\n", e, output); + LOG(1, "ctrl_par error(%f) output(%f)\n", ctrl_par->e, output_f); counter = 0; } } // Set PWM Level and discard results in adc fifo - adjust_pwm_level((uint16_t) roundf(output)); - adc_fifo_drain(); + ctrl_par->output = (uint16_t) roundf(output_f); + adjust_pwm_level(ctrl_par->output); + // Save previous error, integrator, differentiator values + ctrl_par->e_prev = ctrl_par->e; + ctrl_par->i_prev = i; + ctrl_par->d_prev = d; +} - // Check for High or Low Integrator values -> Update x,y values of feedforward function - if ((output < pid->max_output && output > pid->max_output / 2) && - ((i > pid->int_lim_max / 2) || (i < pid->int_lim_min / 2))) { - update_y(pid, i); +// General controller function gets called every x milliseconds where x is CV_49 i.e. sampling time (pid->t) +// TODO: fixed point arithmetic to improve performance... +bool controller_general(struct repeating_timer *const t) { + // Cast (void *) to (controller_parameter_t *) + controller_parameter_t *const ctrl_par = (controller_parameter_t *) (t->user_data); + + // Change in direction -> reset previous derivative, error and integral parts and pwm_base_done + const bool direction_changed = get_direction_of_speed_step(speed_step_target) != + get_direction_of_speed_step(speed_step_target_prev); + // When setpoint == 0 there is nothing to control -> set output to 0, reset values and return + if (!ctrl_par->setpoint || direction_changed) { + ctrl_par->d_prev = 0.0f; + ctrl_par->i_prev = 0.0f; + ctrl_par->e_prev = 0.0f; + ctrl_par->mode = STARTUP_MODE; + ctrl_par->output = 0; + adjust_pwm_level(0); + // return true exits and schedule the repeating timer again + return true; } - // Save previous error, integrator, differentiator, direction, setpoint and measured value also sum error to e_sum - pid->e_prev = e; - pid->i_prev = i; - pid->d_prev = d; - pid->setpoint_prev = pid->setpoint; - pid->measurement_prev = pid->measurement; + // Measure BEMF voltage and compute error + ctrl_par->measurement = measure(ctrl_par->msr_total_iterations, + ctrl_par->msr_delay_in_us, + ctrl_par->l_side_arr_cutoff, + ctrl_par->r_side_arr_cutoff, + get_direction_of_speed_step(speed_step_target)); + ctrl_par->measurement_corrected = ctrl_par->measurement - ctrl_par->adc_offset; + ctrl_par->e = (float) ctrl_par->setpoint - ctrl_par->measurement_corrected; + + switch (ctrl_par->mode){ + case STARTUP_MODE: + controller_startup_mode(ctrl_par); + break; + case PID_MODE: + controller_pid_mode(ctrl_par); + break; + } + + adc_fifo_drain(); + + // Save measurement value + ctrl_par->measurement_prev = ctrl_par->measurement; // return true to schedule the repeating timer again return true; @@ -227,13 +236,21 @@ bool pid_control(struct repeating_timer *const t) { // PID controller and measurement parameter, and speed_table initialization -void init_pid(pid_params *const pid) { +void init_controller(controller_parameter_t *const ctrl_par) { + // General controller variables + ctrl_par->feed_fwd = 0; + ctrl_par->mode = STARTUP_MODE; + ctrl_par->base_pwm_arr_i = 0; + for (int i = 0; i < BASE_PWM_ARR_LEN; ++i) { + ctrl_par->base_pwm_arr[i] = 0; + } + // Measurement variables initialization - pid->adc_offset = (float) CV_ARRAY_FLASH[171]; - pid->msr_total_iterations = CV_ARRAY_FLASH[60]; - pid->msr_delay_in_us = CV_ARRAY_FLASH[61]; - pid->l_side_arr_cutoff = CV_ARRAY_FLASH[62]; - pid->r_side_arr_cutoff = CV_ARRAY_FLASH[63]; + ctrl_par->adc_offset = (float) CV_ARRAY_FLASH[171]; + ctrl_par->msr_total_iterations = CV_ARRAY_FLASH[60]; + ctrl_par->msr_delay_in_us = CV_ARRAY_FLASH[61]; + ctrl_par->l_side_arr_cutoff = CV_ARRAY_FLASH[62]; + ctrl_par->r_side_arr_cutoff = CV_ARRAY_FLASH[63]; // Speed table initialization // Calculate speed setpoint table according to V_min, V_max and V_mid CVs @@ -246,64 +263,55 @@ void init_pid(pid_params *const pid) { const double delta_x = 63; const double m_1 = (v_mid - v_min) / delta_x; const double m_2 = (v_max - v_mid) / delta_x; - pid->speed_table[0] = 0; + ctrl_par->speed_table[0] = 0; for (uint8_t i = 1; i < 64; ++i) { - pid->speed_table[i] = (uint16_t) lround(m_1 * (i - 1) + v_min); + ctrl_par->speed_table[i] = (uint16_t) lround(m_1 * (i - 1) + v_min); } for (uint8_t i = 64; i < 127; ++i) { - pid->speed_table[i] = (uint16_t) lround(m_2 * (i - 63) + v_mid); + ctrl_par->speed_table[i] = (uint16_t) lround(m_2 * (i - 63) + v_mid); } // Controller constants initialization - pid->k_i = (float) CV_ARRAY_FLASH[49] / 10; - pid->k_d = (float) CV_ARRAY_FLASH[50] / 10000; - pid->tau = (float) CV_ARRAY_FLASH[47] / 1000; - pid->t = (float) CV_ARRAY_FLASH[48] / 1000; - pid->ci_0 = (pid->k_i * pid->t) / 2; - pid->cd_0 = -(2 * pid->k_d) / (2 * pid->tau + pid->t); - pid->cd_1 = (2 * pid->tau - pid->t) / (2 * pid->tau + pid->t); - pid->int_lim_max = 10 * (float) CV_ARRAY_FLASH[51]; - pid->int_lim_min = -10 * (float) CV_ARRAY_FLASH[52]; - pid->k_ff = (float) CV_ARRAY_FLASH[46] / 10000; - pid->max_output = (float) (_125M / (CV_ARRAY_FLASH[8] * 100 + 10000)); - pid->x_1_rev = 0.0f; - pid->x_1_fwd = 0.0f; - pid->y_1_fwd = (float) get_16bit_CV(175); - pid->y_1_rev = (float) get_16bit_CV(177); - pid->x_2_rev = (float) pid->speed_table[126]; - pid->x_2_fwd = (float) pid->speed_table[126]; + ctrl_par->k_i = (float) CV_ARRAY_FLASH[49] / 10; + ctrl_par->k_d = (float) CV_ARRAY_FLASH[50] / 10000; + ctrl_par->tau = (float) CV_ARRAY_FLASH[47] / 1000; + ctrl_par->t = (float) CV_ARRAY_FLASH[48] / 1000; + ctrl_par->ci_0 = (ctrl_par->k_i * ctrl_par->t) / 2; + ctrl_par->cd_0 = -(2 * ctrl_par->k_d) / (2 * ctrl_par->tau + ctrl_par->t); + ctrl_par->cd_1 = (2 * ctrl_par->tau - ctrl_par->t) / (2 * ctrl_par->tau + ctrl_par->t); + ctrl_par->int_lim_max = 10 * (float) CV_ARRAY_FLASH[51]; + ctrl_par->int_lim_min = -10 * (float) CV_ARRAY_FLASH[52]; + ctrl_par->max_output = (float) (_125M / (CV_ARRAY_FLASH[8] * 100 + 10000)); // Controller variables initialization - pid->y_2_fwd = pid->max_output; - pid->y_2_rev = pid->max_output; - pid->measurement = 0.0f; - pid->measurement_prev = 0.0f; - pid->e_prev = 0.0f; - pid->i_prev = 0.0f; - pid->d_prev = 0.0f; - pid->setpoint = 0; + ctrl_par->output = 0; + ctrl_par->measurement = 0.0f; + ctrl_par->measurement_prev = 0.0f; + ctrl_par->measurement_corrected = 0.0f; + ctrl_par->e_prev = 0.0f; + ctrl_par->i_prev = 0.0f; + ctrl_par->d_prev = 0.0f; + ctrl_par->setpoint = 0; // Gain scheduling initialization - pid->k_p_x_1_shift = (float) CV_ARRAY_FLASH[59] / 255.0f; - pid->k_p_x_1 = (float) pid->speed_table[126] * pid->k_p_x_1_shift; - pid->k_p_x_2 = (float) pid->speed_table[126] * (1.0f - pid->k_p_x_1_shift); - pid->k_p_y_0 = (float) get_16bit_CV(53) / 100.0f; - pid->k_p_y_1 = (float) get_16bit_CV(55) / 100.0f; - pid->k_p_y_2 = (float) get_16bit_CV(57) / 100.0f; - pid->k_p_m_1 = (pid->k_p_y_1 - pid->k_p_y_0) / pid->k_p_x_1; - pid->k_p_m_2 = (pid->k_p_y_2 - pid->k_p_y_1) / pid->k_p_x_2; - - update_m(pid); + ctrl_par->k_p_x_1_shift = (float) CV_ARRAY_FLASH[59] / 255.0f; + ctrl_par->k_p_x_1 = (float) ctrl_par->speed_table[126] * ctrl_par->k_p_x_1_shift; + ctrl_par->k_p_x_2 = (float) ctrl_par->speed_table[126] * (1.0f - ctrl_par->k_p_x_1_shift); + ctrl_par->k_p_y_0 = (float) get_16bit_CV(53) / 100.0f; + ctrl_par->k_p_y_1 = (float) get_16bit_CV(55) / 100.0f; + ctrl_par->k_p_y_2 = (float) get_16bit_CV(57) / 100.0f; + ctrl_par->k_p_m_1 = (ctrl_par->k_p_y_1 - ctrl_par->k_p_y_0) / ctrl_par->k_p_x_1; + ctrl_par->k_p_m_2 = (ctrl_par->k_p_y_2 - ctrl_par->k_p_y_1) / ctrl_par->k_p_x_2; } void core1_entry() { LOG(1, "core1 init...\n"); - pid_params pid_parameters; - pid_params *pid = &pid_parameters; - init_pid(pid); + controller_parameter_t pid_parameters; + controller_parameter_t *pid = &pid_parameters; + init_controller(pid); alarm_pool_add_repeating_timer_ms(alarm_pool_create(0, 1), CV_ARRAY_FLASH[48], - pid_control, + controller_general, pid, &pid_control_timer); // Set TIMER_IRQ_0 to highest priority of 0x00 @@ -314,5 +322,5 @@ void core1_entry() { pid, &speed_helper_timer); LOG(1, "core1 done\n"); - while (true); + while (true); //sleep_ms(100); //TODO: any potential issues when using sleep? } diff --git a/core1.h b/core1.h index 354e544..7604eae 100644 --- a/core1.h +++ b/core1.h @@ -8,19 +8,54 @@ #pragma once #include "shared.h" +#define BASE_PWM_ARR_LEN 16 + +typedef enum { + STARTUP_MODE, + PID_MODE, +} controller_mode_t; + typedef struct pid_params{ - // Controller variables - won't change after init + // General Controller variables + controller_mode_t mode; + + // Startup mode variables + float feed_fwd; + uint16_t base_pwm_arr[BASE_PWM_ARR_LEN]; + uint16_t base_pwm_arr_i; + + + // PID Controller variables float k_i; // Integral gain float k_d; // Derivative gain float t; // Sampling time float tau; // Low-pass-filter time constant - float k_ff; // Feed-Forward gain float ci_0; // (k_i*t)/2 float cd_0; // (k_d*2)/(2*tau+t) float cd_1; // (2*tau-t)/(2*tau+t) float int_lim_max; // max limit for integrator float int_lim_min; // min limit for integrator float max_output; // max possible pwm output value + float k_p; // Proportional gain + float e; // Error + float e_prev; // Previous Error + float i_prev; // Previous Integral Value + float d_prev; // Previous Derivative Value + uint32_t setpoint; // Current setpoint + uint16_t output; + uint16_t speed_table[127]; // Array with setpoint values corresponding to every speed step + + // Measurement Variables + float measurement; // Newest Measurement value + float measurement_prev; // Previous Measurement + float measurement_corrected; + float adc_offset; // ADC offset value + uint8_t msr_delay_in_us; // Delay before VEMF Measurement + uint8_t msr_total_iterations; // Amount of samples + uint8_t l_side_arr_cutoff; // Discarded samples (left side) + uint8_t r_side_arr_cutoff; // Discarded samples (right side) + + // Variable gain variables float k_p_x_1_shift; float k_p_x_1; float k_p_x_2; @@ -29,41 +64,14 @@ typedef struct pid_params{ float k_p_y_2; float k_p_m_1; float k_p_m_2; - // Controller variables - might change on runtime - float k_p; // Proportional gain - float e_prev; // Previous Error - float i_prev; // Previous Integral Value - float d_prev; // Previous Derivative Value - float measurement_prev; // Previous Measurement - float x_1_fwd; // Base Speed; forward direction - float x_2_fwd; // High Speed; forward direction - float y_1_fwd; // Base PWM; forward direction - float y_2_fwd; // High PWM; forward direction - float x_1_rev; // Base Speed; reverse direction - float x_2_rev; // High Speed; reverse direction - float y_1_rev; // Base PWM; reverse direction - float y_2_rev; // High PWM; reverse direction - float m_fwd; // Slope forward direction - float m_rev; // Slope reverse direction - uint16_t speed_table[127]; // Array with setpoint values corresponding to every speed step - uint32_t speed_step_target; // End target speed step - uint32_t setpoint; // Current setpoint - uint32_t setpoint_prev; // Previous setpoint - // Measurement Variables - float measurement; // Newest Measurement value - float adc_offset; // ADC offset value - uint8_t msr_delay_in_us; // Delay before VEMF Measurement - uint8_t msr_total_iterations; // Amount of samples - uint8_t l_side_arr_cutoff; // Discarded samples (left side) - uint8_t r_side_arr_cutoff; // Discarded samples (right side) -}pid_params; +} controller_parameter_t; + uint8_t get_speed_step_table_index_of_speed_step(uint8_t speed_step); bool speed_helper(struct repeating_timer *t); void adjust_pwm_level(uint16_t level); -void update_m(pid_params *pid); -void update_y(pid_params *pid,float i); -float get_ff_val(const pid_params *pid); -float get_kp(const pid_params *pid); -bool pid_control(struct repeating_timer *t); -void init_pid( pid_params * pid); \ No newline at end of file +float get_kp(const controller_parameter_t *pid); +uint16_t get_initial_level(controller_parameter_t * ctrl_par); +void controller_startup_mode(controller_parameter_t * ctrl_par); +bool controller_general(struct repeating_timer * t); +void init_controller(controller_parameter_t * ctrl_par); \ No newline at end of file diff --git a/shared.c b/shared.c index cee7ed6..745e7cb 100644 --- a/shared.c +++ b/shared.c @@ -15,14 +15,14 @@ float measure(uint8_t total_iterations, uint8_t measurement_delay_us, uint8_t l_side_arr_cutoff, uint8_t r_side_arr_cutoff, - bool direction){ + bool direction) { uint32_t sum = 0; uint16_t adc_val[total_iterations]; int32_t j = 0; uint16_t key; pwm_set_gpio_level(MOTOR_FWD_PIN, 0); pwm_set_gpio_level(MOTOR_REV_PIN, 0); - adc_select_input((!direction)+2); + adc_select_input((!direction) + 2); busy_wait_us(measurement_delay_us); adc_run(true); for (int32_t i = 0; i < total_iterations; ++i) { @@ -39,32 +39,32 @@ float measure(uint8_t total_iterations, adc_run(false); //discard x entries beginning from the lowest entry of the array (counting up); x = msr.l_side_arr_cutoff //discard y entries beginning from the highest index of the array (counting down); y = msr.r_side_arr_cutoff - for (int32_t i = l_side_arr_cutoff; i < total_iterations - r_side_arr_cutoff ; ++i) { + for (int32_t i = l_side_arr_cutoff; i < total_iterations - r_side_arr_cutoff; ++i) { sum += adc_val[i]; } - float res = (float)sum / (float)( total_iterations - (l_side_arr_cutoff + r_side_arr_cutoff) ); + float res = (float) sum / (float) (total_iterations - (l_side_arr_cutoff + r_side_arr_cutoff)); // return true to keep the repeating timer active return res; } // retrieve 4Bytes from CV_ARRAY_FLASH at once and return 32-Bit value -uint32_t get_32bit_CV (uint16_t CV_start_index){ +uint32_t get_32bit_CV(uint16_t CV_start_index) { uint8_t byte_3 = CV_ARRAY_FLASH[CV_start_index]; - uint8_t byte_2 = CV_ARRAY_FLASH[CV_start_index+1]; - uint8_t byte_1 = CV_ARRAY_FLASH[CV_start_index+2]; - uint8_t byte_0 = CV_ARRAY_FLASH[CV_start_index+3]; - return (byte_0) + (byte_1<<8) + (byte_2<<16) + (byte_3<<24); + uint8_t byte_2 = CV_ARRAY_FLASH[CV_start_index + 1]; + uint8_t byte_1 = CV_ARRAY_FLASH[CV_start_index + 2]; + uint8_t byte_0 = CV_ARRAY_FLASH[CV_start_index + 3]; + return (byte_0) + (byte_1 << 8) + (byte_2 << 16) + (byte_3 << 24); } // retrieve 2Bytes from CV_ARRAY_FLASH at once and return 16-Bit value -uint16_t get_16bit_CV (uint16_t CV_start_index){ +uint16_t get_16bit_CV(uint16_t CV_start_index) { uint8_t byte_1 = CV_ARRAY_FLASH[CV_start_index]; - uint8_t byte_0 = CV_ARRAY_FLASH[CV_start_index+1]; - return (byte_0) + (byte_1<<8); + uint8_t byte_0 = CV_ARRAY_FLASH[CV_start_index + 1]; + return (byte_0) + (byte_1 << 8); } -bool get_direction_of_speed_step(uint8_t speed_step){ +bool get_direction_of_speed_step(uint8_t speed_step) { // Shift by 7 Bytes to move bit7 into bit0 position and return return speed_step >> 7; }