Paragliding rewinder control loop

I wrote my first control loop for VESC. It was a pleasure, thanks Benjamin for excellent code.

There is a case in paragliding winching with 2 winches. First winch located near takeoff and second far away about 1.3km. The ropes from first and second winches connected together and to the pilot.

When pilot release rope in the sky, first winch should rewind rope back to takeoff.

First winch should hold about 1-2kg of force when second winch pulls pilot to the sky. And apply 15-20kg after pilot release ropes.

Here is my demo:

https://www.youtube.com/watch?v=D2Q30QTi3Ew

And control loop code:

/*

    Copyright 2019 Kirill Kostiuchenko  kisel2626@gmail.com


    This file is part of the VESC firmware.


    The VESC firmware is free software: you can redistribute it and/or modify

    it under the terms of the GNU General Public License as published by

    the Free Software Foundation, either version 3 of the License, or

    (at your option) any later version.


    The VESC firmware is distributed in the hope that it will be useful,

    but WITHOUT ANY WARRANTY; without even the implied warranty of

    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

    GNU General Public License for more details.


    You should have received a copy of the GNU General Public License

    along with this program.  If not, see <http://www.gnu.org/licenses/>.

    */


#include "app.h"

#include "ch.h"

#include "hal.h"


// Some useful includes

#include "mc_interface.h"

#include "utils.h"

#include "encoder.h"

#include "terminal.h"

#include "comm_can.h"

#include "hw.h"

#include "commands.h"

#include "timeout.h"


#include <math.h>

#include <string.h>

#include <stdio.h>

#include <stdlib.h>


// Threads

static THD_FUNCTION(my_thread, arg);

static THD_WORKING_AREA(my_thread_wa, 2048);


// Private functions

static void brake(int abs_tac);

static void unwinding(int cur_tac);

static void rewinding(int cur_tac);


// Private variables

static volatile bool stop_now = true;

static volatile bool is_running = false;

static int prev_abs_tac = 0;


typedef enum

{

    UNINITIALIZED,

    BRAKING,

    UNWINDING,

    REWINDING

} rewinder_state;


static rewinder_state state;


typedef struct

{

    int brake_length;             // tachometer range for braking

    int overwinding;              // when going back will rewind a bit more then brake_length -> brake_length - overwinding

    int rewinding_trigger_lenght; // start strong rewinding after going back this length

    int unwinding_trigger_length; // go to unwinding state from rewinding, if this length unwinded

    float brake_current;

    float unwinding_current;

    float rewinding_current;

} rewinder_config;


static rewinder_config config;


static char *state_str(void)

{

    switch (state)

    {

    case UNINITIALIZED:

        return "unitialized";

    case BRAKING:

        return "braking";

    case UNWINDING:

        return "unwinding";

    case REWINDING:

        return "rewinding";


    default:

        return "unknown";

    }

}


static void brake(int abs_tac)

{

    commands_printf("rewinder state change: %s -> braking. Abs Tachometer: %d -> %d. Current: %.3fA",

                    state_str(), prev_abs_tac, abs_tac, (double)config.brake_current);


    state = BRAKING;


    prev_abs_tac = abs_tac;

    mc_interface_set_brake_current(config.brake_current);

    timeout_reset();

}


static void unwinding(int cur_tac)

{

    // Detect direction depending on tachometer value

    float current = cur_tac < 0 ? config.unwinding_current : -config.unwinding_current;


    commands_printf("rewinder state change: %s -> unwinding. %.3fA current.", state_str(), (double)current);


    state = UNWINDING;


    mc_interface_set_current(current);

    timeout_reset();

}


static void rewinding(int cur_tac)

{

    // Detect direction depending on tachometer value

    float current = cur_tac < 0 ? config.rewinding_current : -config.rewinding_current;


    commands_printf("rewinder state change: %s -> rewinding. Tachometer: %d. Current: %.3fA",

                    state_str(), cur_tac, (double)current);


    state = REWINDING;


    mc_interface_set_current(current);

    timeout_reset();

}


// Called when the custom application is started. Start our

// threads here and set up callbacks.

void app_custom_start(void)

{

    // Some example lenghts

    config.brake_length = 14;

    config.overwinding = 3;

    config.rewinding_trigger_lenght = 7;

    config.unwinding_trigger_length = 3;

    // Currents

    config.brake_current = 40;

    config.unwinding_current = 7;

    config.rewinding_current = 16;


    state = UNINITIALIZED;


    stop_now = false;


    chThdCreateStatic(my_thread_wa, sizeof(my_thread_wa),

                      NORMALPRIO, my_thread, NULL);


    commands_printf("app_rewinder started");

}


// Called when the custom application is stopped. Stop our threads

// and release callbacks.

void app_custom_stop(void)

{

    stop_now = true;

    while (is_running)

    {

        chThdSleepMilliseconds(1);

    }

    commands_printf("app_rewinder stopped");

}


void app_custom_configure(app_configuration *conf)

{

    (void)conf;

}


static THD_FUNCTION(my_thread, arg)

{

    (void)arg;


    chRegSetThreadName("App Rewinder");


    is_running = true;


    for (int i = 0;; i++)

    {

        // Check if it is time to stop.

        if (stop_now)

        {

            is_running = false;

            return;

        }


        int cur_tac = mc_interface_get_tachometer_value(false);

        int abs_tac = abs(cur_tac);


        switch (state)

        {

        case UNINITIALIZED:

            if (abs_tac < config.brake_length)

                brake(abs_tac);

            else

                unwinding(cur_tac);

            break;

        case BRAKING:

            // We are in the breaking range?

            if (abs_tac <= config.brake_length)

            {

                // Timeout thread will remove breaking every second by default.

                // Apply breaking if position changed

                if (abs_tac != prev_abs_tac)

                {

                    brake(abs_tac);

                }

            }

            else

                unwinding(cur_tac);


            break;

        case UNWINDING:

            // No timeouts for unwinding state

            if (!(i % 5000))

                timeout_reset();


            // We are in the braking range with overwinding?

            if (abs_tac < config.brake_length - config.overwinding)

            {

                brake(abs_tac);

                break;

            }


            // Use prev_abs_tac as max tachometer

            if (abs_tac > prev_abs_tac)

                prev_abs_tac = abs_tac;


            // Going back?

            if (abs_tac < prev_abs_tac - config.rewinding_trigger_lenght)

                rewinding(cur_tac);


            break;

        case REWINDING:

            // No timeouts for rewinding state

            if (!(i % 5000))

                timeout_reset();


            // We are in the braking range with overwinding?

            if (abs_tac < config.brake_length - config.overwinding)

            {

                brake(abs_tac);

                break;

            }


            // Now use prev_abs_tac as min value

            if (abs_tac < prev_abs_tac)

                prev_abs_tac = abs_tac;


            // Unwinding again?

            if (abs_tac > prev_abs_tac + config.unwinding_trigger_length)

                unwinding(cur_tac);


            break;

        default:

            commands_printf("Rewinder: unknown control loop state. Exiting!");

            stop_now = true;

        }


        chThdSleepMicroseconds(1);

    }

}