Pendulum

Implementation using curses

example of controlled

example changing control without recompiling

Dependencies

• ncursesw
• libtcc (from https://repo.or.cz/tinycc.git)

installation

Ubuntu

sudo apt install libncursesw5-dev

git clone https://repo.or.cz/tinycc.git
cd tinycc
./configure
make
sudo make install

Testing

To test our code we need to build it, so we use a simple makefile

# Pendulum
all: pendulum
pendulum: src/pendulum.c
	@gcc $< -Os -lncursesw -lm -ltcc -lpthread -ldl -o $@


run:
	@./pendulum

clean:
	@rm -f pendulum
	@echo "Cleaned"

# end

Run

To test the code we can run «make run».

$TERMINAL -e make run

The Code

First we include some headers we need

#include <stdlib.h>
#include <math.h>
#include <unistd.h>
#include <sys/time.h>
#include <poll.h>
#include <sys/inotify.h>
#include <curses.h>
#include <locale.h>
#include <libtcc.h>
#include <string.h>

Then we define some physics/math constants such as gravity and $π$

#define g -9.8
#define PI 3.14
#define deg(x) x/180.*PI

As we want to watch a file we use inotify events, so we define the sizes of such events and the length of the buffer we are going to use

#define EVENT_SIZE  ( sizeof (struct inotify_event) )
#define BUF_LEN     ( 1024 *( EVENT_SIZE + 16 ) )

We create a preamble of the control file, so the user don’t have to create a function or add math libraries

char preamble[] = "#include <tcclib.h>\n"
  "#include <math.h>\n"
  "#define PI 3.14\n"
  "#define g -9.8\n"
  "typedef struct{\n"
  "    double l,M,m,d;\n"
  "} Sys;\n"
  "typedef struct{\n"
  "double x, dx, a, da;\n"
  "} State;\n"
  "double control(State state,Sys sys,double u,double t)\n"
  "{\n"
  ;

We also add a postamble to close the control function and we include a return, just in case the user forgot to write it.

char postamble[] = "return u;\n"
  "}";

We define the information of the system, such as the length of the rod ($l$), the mass of the cart ($M$), the mass of the rod ($m$) and the damping of the cart ($d$).

typedef struct{
  double l,M,m,d;
} Sys;

We also define the states of the system: cart position ($x$), cart velocity ($\dot{x}$), rod angle ($α$) and rod angular velocity ($\dot{α}$)

typedef struct{
  double x, dx, a, da;
} State;

We create a function to update the time.

double tim(struct timeval * t){
  struct timeval n;
  gettimeofday(&n,0);
  int r=(n.tv_sec-t->tv_sec)*1.e6+n.tv_usec-t->tv_usec;
  *t = n;
  return r / 1.e6;
}

Drawing

To draw the pendulum, we use ncurses.

draw(State state,Sys sys,double u){

We get the maximum values of the terminal

int mx, my;
getmaxyx(stdscr,my,mx);

Then we position the scene using these dimensions

double x=mx/4*state.x+mx/2;
double y=my/2+2;

We scale the length of the rod so we can see it on the screen.

double l=sys.l*my/4;

We then get the cosine and sine of the angle of the rod, as we use it a lot.

double ca=cos(state.a);
double sa=sin(state.a);

Then we clear the screen so we can begin to draw.

erase();

We print some state and control information

mvprintw(0,0,"x=%3.2f m",state.x);
mvprintw(1,0,"ẋ=%3.2f m/s",state.dx);
mvprintw(2,0,"a=%3.2f rad",state.a);
mvprintw(3,0,"ȧ=%3.2f rad/s",state.da);
mvprintw(4,0,"u=%3.2f ",u);

and some controls

mvprintw(0,mx-16,"← to nudge left");
mvprintw(1,mx-16,"→ to nudge right");
mvprintw(2,mx-16,"↵ to restart");

We draw the ground

move((int) y+2,0);
hline(ACS_HLINE, mx);
for(double i=-2;i<2;i+=0.5){
  mvprintw(y+3,mx/4*i+mx/2,"|",i);
  mvprintw(y+4,mx/4*i+mx/2,"%.2f",i);
}

We draw the cart

mvprintw(y-3,x-4,"┌───────┐");
mvprintw(y-2,x-4,"|       │");
mvprintw(y-1,x-4,"|   M   │");
mvprintw(y,x-4  ,"|       │");
mvprintw(y+1,x-4,"└o-----o┘");

We draw the rod

for(double i = 0.1;i<1;i+=0.01){
  double absfa=fabsf(state.a);
  mvprintw(floor(y+i*l*ca),floor(x+i*l*sa),"|");
  if(sa<0&&ca>0.5) mvprintw(floor(y+i*l*ca),floor(x+i*l*sa),"/");
  if(sa>0&&ca>0.5) mvprintw(floor(y+i*l*ca),floor(x+i*l*sa),"\\");
  if(fabsf(ca)<0.1) mvprintw(floor(y+i*l*ca),floor(x+i*l*sa),"-");
}

We draw the mass of the rod

y = floor(y+l*ca);
x = floor(x+l*sa);
mvprintw(y-2,x-2,"");
mvprintw(y-1,x-2,"┌───┐");
mvprintw(y,x-2  ,"| m |");
mvprintw(y+1,x-2,"└───┘");
mvprintw(y+2,x-2,"");

Put all the drawings on the screen.

  refresh();
}

Physics Simulation

For the physics simulation we use equations derived from the Lagrangian formulation of the system

physics(State * state,Sys sys,double dt,double u) {

We create some variables to reduce the cumbersomeness of the equation

double x=state->x;
double dx=state->dx;
double a=state->a;
double da=state->da;

double ca=cos(a);
double sa=sin(a);
double l=sys.l;
double M=sys.M;
double m=sys.m;
double D=m*l*l*(M+m*(1-ca*ca));
double d=sys.d;
double param = m*l*da*da*sa-d*dx;

Here we use a simple Forward Euler to simulate the system. Better integration methods could be used, one that conserves energy for example, but here we don’t want to construct «une usine à gaz».

  double ddx=(1/D)*(-m*m*l*l*g*ca*sa+m*l*l*param)+m*l*l*(1/D)*u;
  state->dx+=ddx*dt;
  state->x+=state->dx*dt;

  double dda = (1/D)*((m+M)*m*g*l*sa-m*l*ca*param)-m*l*ca*(1/D)*u;
  state->da+=dda*dt;
  state->a+=state->da*dt;
}

Reading the file

char* get_fileChars(char* source_file){
  FILE *source;
  int fileSize;
  char *fileChars;

We open the file in read mode

source=fopen(source_file, "r");
if(!source){
  exit(EXIT_FAILURE) ;
}

We go till we find the end of the file just to find its size

fseek(source,0,SEEK_END);
fileSize=ftell(source);

Then we allocate memory to read the file and use the preamble and postamble, and of course add the ‘\0’ at the end of the string. Nota Bene: C strings are null terminated.

int newSize=sizeof(char)*(sizeof(postamble)+sizeof(preamble)+fileSize)+1;
fileChars = (char*) malloc(newSize);
memset(fileChars, '\0', newSize);

Then we copy what we want to memory

strncpy(fileChars, preamble,newSize-1);
fseek(source,0,SEEK_SET);
fread((char*) fileChars+sizeof(preamble)-1,fileSize,1,source);
strcat((char*)fileChars,postamble);

and close the file

  fclose(source);
  return (char*) fileChars;
}

Main Loop

For the main loop

int main(int c, char **v){

We get the current locale, so we can recover. Developing this I bumped in a bug with tcc where if locale is altered, the points of floats are not read and floats are converted into strings, ignoring decimal part.

/* get default locale */
char * locale = setlocale(LC_ALL, 0);

Change locale so we can use unicode characters

setlocale(LC_ALL, "");

We create states for the tcc.

TCCState *tccState;
TCCState *tempTccState;

We configure our inotify watcher

int length, i = 0;
int fd;
char wdbuffer[BUF_LEN];
fd = inotify_init1(IN_NONBLOCK);
if ( fd <0)
{
  fprintf(stderr,"inotify_init");
  exit(EXIT_FAILURE);
}

char source_file[10] = "control.c";

/* Add watch */
int control_watch = inotify_add_watch( fd, source_file, IN_MODIFY);
if (control_watch==-1) {
  fprintf(stderr,"Could not add watch");
  exit(EXIT_FAILURE);
}
struct pollfd pfd = { fd, POLLIN, 0 };
char *fileChars;

We create a function pointer to our control function (not yet defined)

double (*control)(State, Sys,double,double)= 0;

If not defined, we recover the locale, copy files to memory, create a compile context and compile the code.

if(!control){
  setlocale(LC_ALL, locale);
  fileChars = get_fileChars(source_file);
  tccState = tcc_new();
  tcc_set_output_type(tccState, TCC_OUTPUT_MEMORY);
  if(tcc_compile_string(tccState, fileChars)<0){
    exit(EXIT_FAILURE);
  };
  free(fileChars);
  tcc_relocate(tccState, TCC_RELOCATE_AUTO);

Then we get the definition of the symbol and bind with our function pointer.

control = (double (*) (State,Sys,double,double)) tcc_get_symbol(tccState, "control");

Change the locale again (for unicode, it is worth it).

  setlocale(LC_ALL, "");
}

We initialize the system parameters

Sys sys = {2,5,1,1}; // l M m d
double sInit[4] = {-1.5, 0.0, 30.0/180*PI, 0.0}; // x dx α dα
State state = {sInit[0],sInit[1],sInit[2],sInit[3]};

We initialize time

struct timeval t;
gettimeofday(&t, 0);

And also ncurses

initscr();
curs_set(0);

char ch;
double u=0.0;

Set getch to nodelay, so if no key is pressed the system can continue.

if (nodelay(stdscr,TRUE)==ERR){
  return -1;
}

now we begin our main loop

for(;;){

We verify if the control file was modified

/* Verify if control was changed */
int ret = poll(&pfd,1,5);
if (ret > 0)
{
  length = read(fd, wdbuffer, sizeof wdbuffer);
  if (length>0)
  {
    struct inotify_event *watcher_event = ( struct inotify_event * ) &wdbuffer[ i ];
    if ( watcher_event->mask & IN_MODIFY )
    {

If it was, reread file and recompile

      if (watcher_event->wd==control_watch) {
        setlocale(LC_ALL, locale);
        fileChars = get_fileChars(source_file);
        tempTccState = tcc_new();
        tcc_set_output_type(tempTccState, TCC_OUTPUT_MEMORY);
        if(tcc_compile_string(tempTccState, fileChars)<0){
          tcc_delete(tempTccState);
          control = (double (*) (State,Sys,double,double)) tcc_get_symbol(tccState, "control");
        }
        else
        {
          tcc_delete(tccState);
          tcc_relocate(tempTccState, TCC_RELOCATE_AUTO);
          control = (double (*) (State,Sys,double,double)) tcc_get_symbol(tempTccState, "control");
          tccState = tempTccState;
        }
        setlocale(LC_ALL, "");

      }
    }
  }
}

ch = getch();

If a key was pressed, do something

if (ch!=ERR){
  if(ch==68){state.dx-=1;} // nudge left
  if(ch==67){state.dx+=1;} // nudge right
  if(ch==65){
    u=0;
    sInit[0] =0;
    sInit[1] =0;
    sInit[2] =160./180*PI;
    sInit[3] =0;
  }
  if(ch==66){
    sInit[0] =-1.5;
    sInit[1] =0;
    sInit[2] =20.0/180*PI;
    sInit[3] =0;
  }
  if(ch==10){
    u=0;
    state.x =sInit[0];
    state.dx =sInit[1];
    state.a =sInit[2];
    state.da =sInit[3];
  } // Restart
  if(ch==113){
    break;
  }
}

Evaluate control function, apply control value to system and finally draw it.

double time=t.tv_sec;
u=control(state,sys,u,time);
physics(&state,sys,tim(&t),u);
draw(state,sys,u);

Sleep the the sleep of the just

  usleep(20000);
}

Clean the house.

  tcc_delete(tccState);
  endwin();
  return 0;
}