diagram.json

main.py

robo2links

robo2links.chip.json

// robo2links.chip.c
// by dew.ninja  Nov 2023
// This chip implements 2-link manipulator dynamics
// Some timing compensation is coded in chip_init() to adjust
// timing period to match ESP32.
// SPDX-License-Identifier: MIT

#include "wokwi-api.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

static void chip_pin_change(void *user_data, pin_t pin, uint32_t value);
static void chip_spi_done(void *user_data, uint8_t *buffer, uint32_t count);
static void chip_timer_event(void *user_data);  // timer interrupt setup

// double  my_sin(double x);
// double my_cos(double x);

typedef struct {
  pin_t    cs_pin;
  pin_t  reset_pin;
  uint32_t spi;
  uint8_t  spi_buffer[1];
  bool startflag;
  uint32_t tx_linenum;  // send line number to debug
  uint32_t rx_linenum;
  float T;      // sampling period
  uint32_t T_us;  // sampling period in microseconds
  float t_c;   // time compensation for period discrepancy of chip timer
  // link parameters
  // float l1; // link length
  // float m1; // mass
  // float lc1;  // link center of mass
  // float I1; // inertia
  // float l2; // link length
  // float m2; // mass
  // float lc2;  // link center of mass
  // float I2; // inertia  

  float k1;  // k1 = m1*lc1*lc1
  float k2;  // k2 = m2*(l1*l1+lc2*lc2)
  float k3;  // k3 = m2*l1*lc2
  float k4;  // k4 = m2*lc2*lc2
  float k5;  // k5 = (m1*lc1+m2*l1)*g
  float k6;  // k6 = m2*lc2*g
  float I1;
  float I2;
  // -------------
  float i_states[16];  // integrator states
  float d11;  // inertia matrix elements
  float d12;  // d12 = d21
  float d21;
  float d22;
  float g; // gravity
  float g1; // gravity terms
  float g2;
  float b;  //friction
  //float c121 = 0.0, c211=0.0, c221=0.0, c112 = 0.0; // christoffel symbols
  float c11; //corioris matrix elements
  float c12;
  float c21;  
  float zeta;  
  float q1;  // angle of joint 1
  float qd1; // vel of joint 1
  float qdd1; // acc of joint 1
  float q2; // angle of joint 2
  float qd2; // vel of joint 2
  float qdd2; // acc of joint 2
  float tau1;  // torque of joint 1
  float tau2;  // torque of joint 2

  // variables for integration

  float qdd1_1;
  float qd1_1;
  float q1_1;
  float qdd2_1;
  float qd2_1;
  float q2_1;

} chip_state_t;

static void robolinks_setup(chip_state_t *chip);

void chip_init(void) {
  chip_state_t *chip = calloc(1,sizeof(chip_state_t));
  
  chip->cs_pin = pin_init("CS", INPUT_PULLUP);
  chip->reset_pin = pin_init("RESET",INPUT_PULLDOWN);
  float T = 0.05;  // timer period
  chip->T_us = 1e6*T; // period in us
  chip->t_c = 0.1; // chip timer actually runs 10-time faster than setup, so need to compensate
                    //  in the integrator coefficient so that the integrator works correctly.
  printf("timer period = %d us",chip->T_us);
  chip->T = T*chip->t_c;  // period after compensation
  chip->g = 9.81;  // gravity
  chip->tx_linenum = 1;
  chip->startflag = false;  // don't start robot dynamics until first spi received
  const pin_watch_config_t watch_config = {
    .edge = BOTH,
    .pin_change = chip_pin_change,
    .user_data = chip,
  };

  pin_watch(chip->cs_pin, &watch_config);

  const spi_config_t spi_config = {
    .sck = pin_init("SCK", INPUT),
    .miso = pin_init("MISO", INPUT),
    .mosi = pin_init("MOSI", INPUT),
    .done = chip_spi_done,
    .user_data = chip,
  };
  chip->spi = spi_init(&spi_config);
  printf("SPI Chip initialized!\n");
  robolinks_setup(chip);

  const timer_config_t timer_config = {
    .callback = chip_timer_event,
    .user_data = chip,
  };
  timer_t timer_id = timer_init(&timer_config);
  timer_start(timer_id, chip->T_us, true);  
  printf("Timer initialized!\n");

}



// ----------------- another method of integrator implementation -----

// integrator states = [u00,u01,y00,y01,u10,u11,y10,y11, ...]
// ui0, ui1 refer to u0, u1 of channel i
// channel 0,1,2,3 = q1, q2, qd1, qd2



void integrator_states_reset(chip_state_t *chip)
{
  chip->qdd1_1=0.0;
  chip->qd1_1=0.0;
  chip->q1_1=0.0;
  chip->qdd2_1=0.0;
  chip->qd2_1=0.0;
  chip->q2_1=0.0;
}

// function y = intg8r_compute(u, ch)
// usage : q1 = intg8r_compute(qd1, 0)
// float itg8r_compute(chip_state_t *chip,float u, int ch)
// {
//   chip->i_states[4*ch+1]=chip->i_states[4*ch]; // u1 = u0
//   chip->i_states[4*ch] = u;                   // u0 = u
//   chip->i_states[4*ch+3] = chip->i_states[4*ch+2];  // y1 = y0
//   // y0 = y1 + 0.5*T*(u0+u1)
//   // bilinear transform
//   chip->i_states[4*ch+2] = chip->i_states[4*ch+3] + 0.5*chip->T*(chip->i_states[4*ch]+chip->i_states[4*ch+1]);
//   // backward difference
//   //chip->i_states[4*ch+2] = chip->i_states[4*ch+3] + chip->T*chip->i_states[4*ch];
//   // forward difference
//   //chip->i_states[4*ch+2] = chip->i_states[4*ch+3] + chip->T*chip->i_states[4*ch+1];
  
//   return chip->i_states[4*ch+2];
// }


void robolinks_setup(chip_state_t *chip)
{

  float l1 = 1.0;
  float m1 = 2.2; 
  float lc1 = 0.5*l1;
  float I1 = 2.0;

  float l2 = 1.0;
  float m2 = 1.8; //2.0;
  float lc2 = 0.5*l2;
  float I2 = 1.5;  
  float g = 9.81; 
  chip->b = 0.1;  // friction
  
  chip->k1 = m1*lc1*lc1;
  chip->k2 = m2*(l1*l1 + lc2*lc2);
  chip->k3 = m2*l1*lc2;
  chip->k4 = m2*lc2*lc2;
  chip->k5 = (m1*lc1 +m2*l1)*g;
  chip->k6 = m2*lc2*g;

  chip->I1 = I1;
  chip->I2 = I2;

  chip->d22 = chip->k4 + I2;  // this term is constant
  // printf("d22 = %f\n",d22);
  chip->q1 = 0.0;
  chip->qd1 = 0.0;
  chip->qdd1 = 0.0;
  chip->q2 = 0.0;
  chip->qd2 = 0.0;
  chip->qdd2 = 0.0;
  integrator_states_reset(chip);
  printf("Manipulator setup completed\n");

}


uint8_t str2send[255];
//uint8_t *str2send;
uint8_t idx = 0;
char input_buffer[255];
uint8_t ib_idx = 0;
char tau1_str[80], tau2_str[80];
char rx_linebuf[10];



void split_inbuf(chip_state_t *chip)  {
  char *token = strtok(input_buffer, ",");
  sprintf((char *)rx_linebuf,"%s\n",token);
  token = strtok(NULL, ",");
  sprintf((char *)tau1_str,"%s\n",token);
  token = strtok(NULL, "\n");
  sprintf((char *)tau2_str,"%s\n",token);
}



// robot dynamics update is done here -----
void chip_timer_event(void *user_data) {
  chip_state_t *chip = (chip_state_t*)user_data;
  uint32_t reset = pin_read(chip->reset_pin);  // reset button pressed

  double cos1 = cos(chip->q1); //precompute terms used in more than one place
  double cos2 = cos(chip->q2);
  //float sin2 = sin(chip->q2);
  double sin2 = sin(chip->q2);

  float x1 = chip->k3*cos2;
  float x2 = chip->k3*sin2;

  //printf("q1 = %f, cos_q1 = %f\n",chip->q1,cos_q1);
  // inertia matrix elements

  chip->d11 = chip->k1 + chip->k2 + 2*x1 + chip->I1 + chip->I2;
  chip->d12 = chip->k4 + 2*x1 + chip->I2;
  chip->d21 = chip->d12;
  // chip->d11 = chip->m1*chip->lc1*chip->lc1 + chip->m2*(chip->l1*chip->l1 
  //   + chip->lc2*chip->lc2 + 2*chip->l1*chip->lc2*cos_q2)+chip->I1+chip->I2;
  // chip->d12 = chip->m2*(chip->lc2*chip->lc2 + chip->l1*chip->lc2*cos_q2)+ chip->I2;
  // chip->d21 = chip->d12;
  //printf("d11 = %f, d12 = d21 = %f, d22 = %f\n",chip->d11,chip->d12,chip->d22);

  // gravity terms
  double cos12 = cos(chip->q1 + chip->q2);
  chip->g2 = chip->k6*cos12;
  chip->g1 = chip->k5*cos1 + chip->g2;
  // chip->g1 = (chip->m1*chip->lc1 + chip->m2*chip->l1)*chip->g*cos_q1 
  //         + chip->m2*chip->lc2*chip->g*cos12;
  // chip->g2 = chip->m2*chip->lc2*chip->g*cos12;
  //printf("g1 = %f, g2 = %f\n",chip->g1,chip->g2);

  // corioris matrix 
  chip->c11 = -x2*chip->qd2;
  chip->c12 = -x2*(chip->qd1 + chip->qd2);
  chip->c21 = x2*chip->qd1;


  // chip->zeta = -chip->m2*chip->l1*chip->lc2*sin_q2;
  // chip->c11 = chip->zeta*chip->qd2;
  // chip->c12 = chip->zeta*(chip->qd1+chip->qd2);
  // chip->c21 = -chip->zeta*chip->qd1;
  //printf("c11 = %f, c12 = %f, c21 = %f\n",chip->c11,chip->c12,chip->c21);
  //c22 = 0;
  chip->qdd1_1 = chip->qdd1;  // used for integration
  chip->qdd2_1 = chip->qdd2; 
  chip->qdd1 = (chip->tau1 - chip->d12*chip->qdd2 - (chip->c11+chip->b)*chip->qd1
        - chip->c12*chip->qd2 -chip->g1)/chip->d11;  
  chip->qdd2 = (chip->tau2 - chip->d21*chip->qdd1 - chip->c21*chip->qd1 -chip->b*chip->qd2 
        - chip->g2)/chip->d22;
  printf("qdd1 = %f\n",chip->qdd1);
  // chip->qd1 = itg8r_compute(chip,chip->qdd1,2);
  // chip->qd2 = itg8r_compute(chip,chip->qdd2,3);
  // chip->q1 = itg8r_compute(chip,chip->qd1,0);
  // chip->q2 = itg8r_compute(chip,chip->qd2,1);

  chip->qd1_1 = chip->qd1;
  chip->qd2_1 = chip->qd2;

  // integrate acceleration to get velocity using bilinear transform
  chip->qd1 = chip->qd1_1 + 0.5*chip->T*(chip->qdd1_1 + chip->qdd1);
  chip->qd2 = chip->qd2_1 + 0.5*chip->T*(chip->qdd2_1 + chip->qdd2);

  chip->q1_1 = chip->q1;
  chip->q2_1 = chip->q2;

  // integrate velocity to get angle using bilinear transform
  chip->q1 = chip->q1_1 + 0.5*chip->T*(chip->qd1_1 + chip->qd1);
  chip->q2 = chip->q2_1 + 0.5*chip->T*(chip->qd2_1 + chip->qd2);


  // printf("q1 = %f,qd1 = %f,qdd1 = %f\n",chip->q1,chip->qd1,chip->qdd1);
  // printf("q2 = %f,qd2 = %f,qdd2 = %f\n",chip->q2,chip->qd2,chip->qdd2);
  // printf("tau1 = %f,tau2 = %f\n",chip->tau1,chip->tau2);
  // printf("--------------------------\n");
  if (!chip->startflag)  { // wait until chip->startflag is true
    reset = 1;
  }

  if (reset) robolinks_setup(chip);


}


uint8_t send_str(uint8_t *str2send, uint8_t i)   {
  return str2send[i];

}

void chip_pin_change(void *user_data, pin_t pin, uint32_t value) {
  chip_state_t *chip = (chip_state_t*)user_data;
  // Handle CS pin logic
  if (pin == chip->cs_pin) {
    if (value == LOW) {
      printf("SPI chip selected\n");
      if (!chip->startflag)  {
        chip->startflag = true;
        printf("Manipulator dynamics started\n");
      }
      chip->spi_buffer[0] = ' '; // Some dummy data for the first character
      
      sprintf((char *)str2send,"%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,\n",chip->tx_linenum,chip->q1,chip->qd1,chip->qdd1,chip->q2,chip->qd2,chip->qdd2); 
      printf("%s",str2send);
      chip->tx_linenum++;
      if (chip->tx_linenum>32768) chip->tx_linenum=1;
      spi_start(chip->spi, chip->spi_buffer, sizeof(chip->spi_buffer));
    } else {
      printf("SPI chip deselected\n");
      printf("input_buffer (torque) is %s",input_buffer);
      idx = 0;
      ib_idx = 0;
      split_inbuf(chip);
      chip->tau1 = atof(tau1_str);
      chip->tau2 = atof(tau2_str);
      chip->rx_linenum = atoi(rx_linebuf);
      printf("Line: %d : tau1 = %f,tau2 = %f\n",chip->rx_linenum, chip->tau1,chip->tau2);      

      spi_stop(chip->spi);
    }
  }
}

void chip_spi_done(void *user_data, uint8_t *buffer, uint32_t count) {
  chip_state_t *chip = (chip_state_t*)user_data;
  if (!count) {
    // This means that we got here from spi_stop, and no data was received
    return;
  }


  //printf("%c",buffer[0]);
  //strncat(input_buffer, (char *) buffer,1);
  input_buffer[ib_idx++] = buffer[0];
  buffer[0] = send_str(str2send,idx++);

  if (pin_read(chip->cs_pin) == LOW) {
    // Continue with the next character
    spi_start(chip->spi, chip->spi_buffer, sizeof(chip->spi_buffer));
  }
}

// // math function to compute sin() and cos()

// // In this function, we'll recursively calculate the factorial of a number
// int factorial(int n) {
//   if (n == 0) {
//     return 1;
//   }

//   return n * factorial(n - 1);
// }

// // Calculation of a number's power
// double mypow(double base, int exponent) {
//   double result = 1;
//   for (int i = 0; i < exponent; i++) {
//     result *= base;
//   }

//   return result;
// }

// // We need to know the sign of the number
// double sign(double x) {
//   if (x > 0) {
//     return 1;
//   } else if (x < 0) {
//     return -1;
//   } else {
//     return 0;
//   }
// }

// // Calculating sine using the above Taylor series
// double my_sin(double x) {
//   // Our implementation only works for numbers from -2*M_PI to 2*M_PI
//   // Therefore, we use the fact that sin(x) == sin(x + 2*M_PI) for any x
//   while (fabs(x) >= 2 * M_PI) {
//     x -= sign(x) * 2 * M_PI;
//   }

//   double result = x;
//   // Here we calculate 4 terms of the series
//   // We can compute more terms for better precision
//   for (int i = 1; i <= 4; i++) {
//     double term = (double)mypow(x, 1 + 2 * i) / factorial(1 + 2 * i);
//     if (i % 2 == 1) {
//       result -= term;
//     } else {
//       result += term;
//     }
//   }
//   return result;
// }

// // Calculating cos using Taylor series
// // cos(x) = 1 - x^2/2! + x^4/4! - x^6/6! 
// double my_cos(double x) {
//   // Our implementation only works for numbers from -2*M_PI to 2*M_PI
//   // Therefore, we use the fact that sin(x) == sin(x + 2*M_PI) for any x
//   while (fabs(x) >= 2 * M_PI) {
//     x -= sign(x) * 2 * M_PI;
//   }

//   //double result = x;
//   double result = 1;

//   // Here we calculate 4 terms of the series
//   // We can compute more terms for better precision
//   for (int i = 1; i <= 4; i++) {
//     double term = (double)mypow(x, 2 * i) / factorial(2 * i);
//     if (i % 2 == 1) {
//       result -= term;
//     } else {
//       result += term;
//     }
//   }
//   return result;
// }
