Anti-Sway Control Law Implementation. More...

#include <stdbool.h>
#include <pthread.h>
#include <stdint.h>
#include <stdlib.h>
#include <math.h>
#include "setup.h"
#include "io.h"
#include "thread-lib.h"
#include "discrete-lib.h"
#include "record.h"
#include "anti-sway.h"

Include dependency graph for anti-sway.c:

Data Structures
struct	AntiSwayControlScheme
	Anti-Sway Mode Feedback Control Block. More...

Macros
#define	DATA_LEN 20
	The number of entries.

#define	TUNING
	Tuning Mode.

#define	TUNING_DATA_LEN 10
	The number of array entries within the tuning file.

#define	LR_X 250
	Learning Rate in X direction.

#define	LR_Y 250
	Learning Rate in Y direction.

#define	ZERO_GRAD()

Functions
static void	SetupScheme (AntiSwayControlScheme *scheme, Proportional K_p, Proportional K_i, Proportional m)
	Sets up the Anti-Sway Control Law (its feedback path)

static void *	AntiSwayModeThread (void *resource)
	Runs Anti-Sway.

static int	AntiSwayControlLaw (Velocity vel_ref, Angle angle_input, Velocity vel_input, AntiSwayControlScheme scheme, int(SetVoltage)(Voltage voltage))
	Executes an iteration of the feedback path for Anti-Sway.

int	AntiSwayFork ()
	Executes Anti-Sway Mode.

int	AntiSwayJoin ()
	Stops Anti-Sway Mode.

Variables
pthread_t	anti_sway_thread = NULL
	Thread ID.

ThreadResource	anti_sway_resource
	Thread Resources (Shared Resources)

static double	K_ptx = 51.55550206284189
	The proportional constant for inner-loop.

static double	K_itx = 33.28586146285062
	The integral constant for inner-loop control.

static double	K_pty = 0.8*55.65965893434064
	The proportional constant for inner-loop.

static double	K_ity = 0.8*31.59324977878787
	The integral constant for inner-loop control.

static AntiSwayControlScheme	x_control
	The Control Scheme for the X Motor.

static AntiSwayControlScheme	y_control
	The Control Scheme for the Y Motor.

static int	error
	Local Error Code.

static FileID_t	file = -1
	The file ID.

static char *	data_file_name = "anti-sway.mat"
	The file Name.

static char *	data_names [DATA_LEN]
	The data names.

static double	data [DATA_LEN]
	Buffer for data.

static double *	data_buff = data
	Pointer to next data point to insert into buffer.

static int	id = 1
	ID variable.

static double	t = 0.0
	timestamp

static FileID_t	tuning_file = -1
	The tuning file.

static char *	tuning_file_name = "anti-sway-tuning.mat"
	The name of the tuning file.

static char *	tuning_data_names [TUNING_DATA_LEN]
	The names of the array entries for the tuning file.

static double	dKp [2]

static double	dKi [2]

static int	total_pts [2]

static double	prev_int_Kp [2][550]

static double	prev_int_Ki [2][550]

static int	prev_int_i = 0

static bool	int_Kp_first = true
	Indicates (false) if prev_int_Kp has any valid data in it.

static bool	int_Ki_first = true
	Indicates (false) if prev_int_Ki has any valid data in it.

static double	prev_Kp [2]
	Stores previous Kp values in both x and y directions.

static double	prev_Ki [2]
	Stores previous Ki value sin both x and y directions.

Detailed Description

Anti-Sway Control Law Implementation.

Author: Anti-Sway Team: Nguyen, Tri; Espinola, Malachi; Tevy, Vattanary; Hokenstad, Ethan; Neff, Callen)

Version: 0.1

Date: 2024-06-03

Copyright: Copyright (c) 2024

Macro Definition Documentation

◆ ZERO_GRAD

#define ZERO_GRAD ( )

Value:

    dKp[0] = 0.0; \
    dKp[1] = 0.0; \
    dKi[0] = 0.0; \
    dKi[1] = 0.0; \
    total_pts[0] = 0; \
    total_pts[1] = 0;

Zeros out Gradients

Postcondition: Zeros out dKp and dKi

Function Documentation

◆ AntiSwayControlLaw()

static int AntiSwayControlLaw	(	Velocity	vel_ref,
		Angle	angle_input,
		Velocity	vel_input,
		AntiSwayControlScheme *	scheme,
		int(*	SetVoltage )(Voltage voltage) )

inlinestatic

Executes an iteration of the feedback path for Anti-Sway.

Executes 1 timestep for the Anti-Sway Mode Control Law for its input to the plant

Parameters

vel_ref	The reference velocity for Anti-Sway Mode
angle_input	The measured rope angle for Anti-Sway Mode
vel_input	The measured velocity of the motor
scheme	A pointer to the AntiSwayControlScheme structure used to execute the control law
SetVoltage	The function that sets the voltage of the appropriate motor

Returns: 0 upon success, negative otherwise

Precondition: scheme was not modified before use of this function

Postcondition: scheme is now updated with the input and outputs for the respective control scheme

Here is the call graph for this function:

◆ AntiSwayFork()

int AntiSwayFork ( )

Executes Anti-Sway Mode.

Executes Anti-Sway Mode (concurrently)

Precondition: Anti-Sway Mode is not already running

Here is the call graph for this function:

◆ AntiSwayJoin()

int AntiSwayJoin ( )

Stops Anti-Sway Mode.

Stops Anti-Sway Mode (concurrent process)

Here is the call graph for this function:

◆ AntiSwayModeThread()

static void * AntiSwayModeThread ( void * resource )

static

Runs Anti-Sway.

The Thread Function for Anti-Sway Mode

Parameters

resource A pointer to a Resource sturcture for Tracking Mode

Returns: NULL

Here is the call graph for this function:

◆ SetupScheme()

static void SetupScheme	(	AntiSwayControlScheme *	scheme,
		Proportional	K_p,
		Proportional	K_i,
		Proportional	m )

inlinestatic

Sets up the Anti-Sway Control Law (its feedback path)

Sets up an AntiSwayControlScheme

Parameters

scheme	The scheme to setup
K_p	The proportional gain
K_i	The integral gain
m	The combined masses

Postcondition: scheme is now setup with zero initial conditions and proper constants

Here is the call graph for this function:

Variable Documentation

◆ data_names

char* data_names[DATA_LEN]

static

Initial value:

= {"id", "t",
                                     "vel_ref_x", "vel_ref_y",
                                     "angle_x", "angle_y",
                                     "trolley_vel_x", "trolley_vel_y",
                                     "vel_err_x", "voltage_x", "int_out_x", "Kp_x'", "Ki_x'", "loss_x",
                                     "vel_err_y", "voltage_y", "int_out_y", "Kp_y'", "Ki_y'", "loss_y"}

The data names.

◆ dKi

double dKi[2]

static

The gradient component of the loss with respect to Ki, for both x and y directions

◆ dKp

double dKp[2]

static

The gradient component of the loss with respect to Kp, for both x and y directions

◆ prev_int_i

int prev_int_i = 0

static

The counter that tells the program where we are along a column within above 2 arrays

◆ prev_int_Ki

double prev_int_Ki[2][550]

static

Previous integral outputs with respect to change in Ki, for both x and y directions

◆ prev_int_Kp

double prev_int_Kp[2][550]

static

Previous integral outputs with respect to change in Kp, for both x and y directions

◆ total_pts

int total_pts[2]

static

The total number of data points used for dKp and dKi, for both x and y directions

◆ tuning_data_names

char* tuning_data_names[TUNING_DATA_LEN]

static

Initial value:

= {"count_x", "count_y",
                                                   "dKp_x", "dKi_x", "dKp_y", "dKi_y",
                                                   "Kp_x", "Ki_x", "Kp_y", "Ki_y"}

The names of the array entries for the tuning file.

Data Structures

Macros

Functions

Variables

Detailed Description

Macro Definition Documentation

◆ ZERO_GRAD

Function Documentation

◆ AntiSwayControlLaw()

◆ AntiSwayFork()

◆ AntiSwayJoin()

◆ AntiSwayModeThread()

◆ SetupScheme()

Variable Documentation

◆ data_names

◆ dKi

◆ dKp

◆ prev_int_i

◆ prev_int_Ki

◆ prev_int_Kp

◆ total_pts

◆ tuning_data_names