<commit_before>#include <string.h>
#include <cocobot.h>
#include <platform.h>
#include <FreeRTOS.h>
#include <event_groups.h>
#include <stm32f4xx.h>
#include "meca_crimp.h"

#include <cocobot/asserv_pid.h>

//max 1050 (ferme)
//min 688 (ouvert)

#define MECA_CRIMP_MAX_OPENNING 100

#define MECA_CRIMP_LEFT_SERVO_ID         PLATFORM_SERVO_15_ID
#define MECA_CRIMP_RIGHT_SERVO_ID        PLATFORM_SERVO_3_ID
#define MECA_CRIMP_VERTICAL_SERVO_ID     PLATFORM_SERVO_14_ID

#define MECA_CRIMP_LEFT_SERVO_OPEN 290
#define MECA_CRIMP_LEFT_SERVO_CLOSE 100
#define MECA_CRIMP_LEFT_SERVO_DISABLE 0

#define MECA_CRIMP_RIGHT_SERVO_OPEN 340
#define MECA_CRIMP_RIGHT_SERVO_CLOSE 500
#define MECA_CRIMP_RIGHT_SERVO_DISABLE 0

#define MECA_CRIMP_VERTICAL_SERVO_GROUND 0
#define MECA_CRIMP_VERTICAL_SERVO_FENCE 0
#define MECA_CRIMP_VERTICAL_SERVO_F1 0
#define MECA_CRIMP_VERTICAL_SERVO_DISABLE 0

static unsigned int servo_left_set_point;
static unsigned int servo_right_set_point;
static unsigned int servo_vertical_set_point;

static volatile int crimp_disable;
static volatile int crimp_current;
static volatile int crimp_target;
static volatile int32_t last_output;

static cocobot_asserv_pid_t pid;

static void meca_crimp_update_servo(void)
{
  platform_servo_set_value(MECA_CRIMP_LEFT_SERVO_ID, servo_left_set_point);
  platform_servo_set_value(MECA_CRIMP_RIGHT_SERVO_ID, servo_right_set_point);
  platform_servo_set_value(MECA_CRIMP_VERTICAL_SERVO_ID, servo_vertical_set_point);
}

static void meca_crimp_task(void * arg)
{
  (void)arg;
  while(1)
  {
    crimp_current = platform_adc_get_mV(PLATFORM_ADC_CH0);

    if(crimp_disable)
    {
      platform_gpio_clear(PLATFORM_GPIO1);
      cocobot_asserv_pid_reset(&pid);
    }
    else
    {
      platform_gpio_set(PLATFORM_GPIO1);
      cocobot_asserv_pid_set_input(&pid, crimp_target);
      cocobot_asserv_pid_set_feedback(&pid, crimp_current);

      cocobot_asserv_pid_compute(&pid);

      int32_t output = cocobot_asserv_pid_get_output(&pid);
      last_output = output;
      cocobot_asserv_pid_compute(&pid);

      if(output > 0)
      {
        if(output > 0xFFFF)
        {
          output = 0xFFFF;
        }

        platform_gpio_clear(PLATFORM_GPIO6);
        mcual_timer_set_duty_cycle(MCUAL_TIMER1, MCUAL_TIMER_CHANNEL4, output);
      }
      else
      {
        output = -output;
        if(output > 0xFFFF)
        {
          output = 0xFFFF;
        }
        platform_gpio_set(PLATFORM_GPIO6);
        mcual_timer_set_duty_cycle(MCUAL_TIMER1, MCUAL_TIMER_CHANNEL4, 0xFFFF - output);
      }
    }
    
    vTaskDelay(50 / portTICK_PERIOD_MS);
  }
}

void meca_crimp_init(void)
{


  servo_left_set_point = MECA_CRIMP_LEFT_SERVO_DISABLE;
  servo_right_set_point = MECA_CRIMP_RIGHT_SERVO_DISABLE;
  servo_vertical_set_point = MECA_CRIMP_VERTICAL_SERVO_DISABLE;

  platform_gpio_set_direction(PLATFORM_GPIO1, MCUAL_GPIO_OUTPUT);
  platform_gpio_set_direction(PLATFORM_GPIO6, MCUAL_GPIO_OUTPUT);
  platform_gpio_set_direction(PLATFORM_GPIO7, MCUAL_GPIO_OUTPUT);

  platform_gpio_clear(PLATFORM_GPIO1);
  platform_gpio_clear(PLATFORM_GPIO6);
  platform_gpio_clear(PLATFORM_GPIO7);

  mcual_timer_init(MCUAL_TIMER1, 20000);
  mcual_timer_set_duty_cycle(MCUAL_TIMER1, MCUAL_TIMER_CHANNEL4, 0x0000);
  mcual_timer_enable_channel(MCUAL_TIMER1, MCUAL_TIMER_CHANNEL4);
  mcual_gpio_set_function(MCUAL_GPIOE, MCUAL_GPIO_PIN14, 1);

  platform_gpio_clear(PLATFORM_GPIO1);

  cocobot_asserv_pid_init(&pid);
  cocobot_asserv_pid_set_kp(&pid, 750.0);
  cocobot_asserv_pid_set_kd(&pid, 1000.0);


  meca_crimp_set_target(750);
  crimp_disable = 0;

  xTaskCreate(meca_crimp_task, "crimp", 200, NULL, 2, NULL);

  //meca_crimp_close();
}

void meca_crimp_open(void)
{
  servo_left_set_point = MECA_CRIMP_LEFT_SERVO_OPEN;
  servo_right_set_point = MECA_CRIMP_RIGHT_SERVO_OPEN;

  meca_crimp_update_servo();
}

void meca_crimp_close(void)
{
  servo_right_set_point = MECA_CRIMP_RIGHT_SERVO_CLOSE;
  meca_crimp_update_servo();
  vTaskDelay(150 / portTICK_PERIOD_MS);

  servo_left_set_point = MECA_CRIMP_LEFT_SERVO_CLOSE;
  meca_crimp_update_servo();
}

void meca_crimp_disable(void)
{
  //crimp_target = 891;//MECA_CRIMP_MAX_OPENNING;
  crimp_disable = 1;

  servo_left_set_point = MECA_CRIMP_LEFT_SERVO_DISABLE;
  servo_right_set_point = MECA_CRIMP_RIGHT_SERVO_DISABLE;
  servo_vertical_set_point = MECA_CRIMP_VERTICAL_SERVO_DISABLE;
  meca_crimp_update_servo();
}

int meca_crimp_console_handler(const char * command)
{
  if(strcmp(command,"meca_crimp_left_servo") == 0)
  {
    int set;
    if(cocobot_console_get_iargument(0, &set))
    {
      servo_left_set_point = set;
      meca_crimp_update_servo();
    }
    cocobot_console_send_answer("%u", servo_left_set_point);
    return 1;<commit_msg>close door at before starter removed<commit_after>#include <string.h>
#include <cocobot.h>
#include <platform.h>
#include <FreeRTOS.h>
#include <event_groups.h>
#include <stm32f4xx.h>
#include "meca_crimp.h"

#include <cocobot/asserv_pid.h>

//max 1050 (ferme)
//min 688 (ouvert)

#define MECA_CRIMP_MAX_OPENNING 100

#define MECA_CRIMP_LEFT_SERVO_ID         PLATFORM_SERVO_15_ID
#define MECA_CRIMP_RIGHT_SERVO_ID        PLATFORM_SERVO_3_ID
#define MECA_CRIMP_VERTICAL_SERVO_ID     PLATFORM_SERVO_14_ID

#define MECA_CRIMP_LEFT_SERVO_OPEN 200
#define MECA_CRIMP_LEFT_SERVO_CLOSE 550
#define MECA_CRIMP_LEFT_SERVO_DISABLE 0

#define MECA_CRIMP_RIGHT_SERVO_OPEN 340
#define MECA_CRIMP_RIGHT_SERVO_CLOSE 500
#define MECA_CRIMP_RIGHT_SERVO_DISABLE 0

#define MECA_CRIMP_VERTICAL_SERVO_GROUND 0
#define MECA_CRIMP_VERTICAL_SERVO_FENCE 0
#define MECA_CRIMP_VERTICAL_SERVO_F1 0
#define MECA_CRIMP_VERTICAL_SERVO_DISABLE 0

static unsigned int servo_left_set_point;
static unsigned int servo_right_set_point;
static unsigned int servo_vertical_set_point;

static volatile int crimp_disable;
static volatile int crimp_current;
static volatile int crimp_target;
static volatile int32_t last_output;

static cocobot_asserv_pid_t pid;

static void meca_crimp_update_servo(void)
{
  platform_servo_set_value(MECA_CRIMP_LEFT_SERVO_ID, servo_left_set_point);
  platform_servo_set_value(MECA_CRIMP_RIGHT_SERVO_ID, servo_right_set_point);
  platform_servo_set_value(MECA_CRIMP_VERTICAL_SERVO_ID, servo_vertical_set_point);
}

static void meca_crimp_task(void * arg)
{
  (void)arg;
  while(1)
  {
    crimp_current = platform_adc_get_mV(PLATFORM_ADC_CH0);

    if(crimp_disable)
    {
      platform_gpio_clear(PLATFORM_GPIO1);
      cocobot_asserv_pid_reset(&pid);
    }
    else
    {
      platform_gpio_set(PLATFORM_GPIO1);
      cocobot_asserv_pid_set_input(&pid, crimp_target);
      cocobot_asserv_pid_set_feedback(&pid, crimp_current);

      cocobot_asserv_pid_compute(&pid);

      int32_t output = cocobot_asserv_pid_get_output(&pid);
      last_output = output;
      cocobot_asserv_pid_compute(&pid);

      if(output > 0)
      {
        if(output > 0xFFFF)
        {
          output = 0xFFFF;
        }

        platform_gpio_clear(PLATFORM_GPIO6);
        mcual_timer_set_duty_cycle(MCUAL_TIMER1, MCUAL_TIMER_CHANNEL4, output);
      }
      else
      {
        output = -output;
        if(output > 0xFFFF)
        {
          output = 0xFFFF;
        }
        platform_gpio_set(PLATFORM_GPIO6);
        mcual_timer_set_duty_cycle(MCUAL_TIMER1, MCUAL_TIMER_CHANNEL4, 0xFFFF - output);
      }
    }
    
    vTaskDelay(50 / portTICK_PERIOD_MS);
  }
}

void meca_crimp_init(void)
{


  servo_left_set_point = MECA_CRIMP_LEFT_SERVO_DISABLE;
  servo_right_set_point = MECA_CRIMP_RIGHT_SERVO_DISABLE;
  servo_vertical_set_point = MECA_CRIMP_VERTICAL_SERVO_DISABLE;

  platform_gpio_set_direction(PLATFORM_GPIO1, MCUAL_GPIO_OUTPUT);
  platform_gpio_set_direction(PLATFORM_GPIO6, MCUAL_GPIO_OUTPUT);
  platform_gpio_set_direction(PLATFORM_GPIO7, MCUAL_GPIO_OUTPUT);

  platform_gpio_clear(PLATFORM_GPIO1);
  platform_gpio_clear(PLATFORM_GPIO6);
  platform_gpio_clear(PLATFORM_GPIO7);

  mcual_timer_init(MCUAL_TIMER1, 20000);
  mcual_timer_set_duty_cycle(MCUAL_TIMER1, MCUAL_TIMER_CHANNEL4, 0x0000);
  mcual_timer_enable_channel(MCUAL_TIMER1, MCUAL_TIMER_CHANNEL4);
  mcual_gpio_set_function(MCUAL_GPIOE, MCUAL_GPIO_PIN14, 1);

  platform_gpio_clear(PLATFORM_GPIO1);

  cocobot_asserv_pid_init(&pid);
  cocobot_asserv_pid_set_kp(&pid, 750.0);
  cocobot_asserv_pid_set_kd(&pid, 1000.0);


  meca_crimp_set_target(750);
  crimp_disable = 0;

  xTaskCreate(meca_crimp_task, "crimp", 200, NULL, 2, NULL);

  meca_crimp_close();
}

void meca_crimp_open(void)
{
  servo_left_set_point = MECA_CRIMP_LEFT_SERVO_OPEN;
  servo_right_set_point = MECA_CRIMP_RIGHT_SERVO_OPEN;

  meca_crimp_update_servo();
}

void meca_crimp_close(void)
{
  servo_right_set_point = MECA_CRIMP_RIGHT_SERVO_CLOSE;
  servo_left_set_point = MECA_CRIMP_LEFT_SERVO_CLOSE;
  meca_crimp_update_servo();
}

void meca_crimp_disable(void)
{
  //crimp_target = 891;//MECA_CRIMP_MAX_OPENNING;
  crimp_disable = 1;

  servo_left_set_point = MECA_CRIMP_LEFT_SERVO_DISABLE;
  servo_right_set_point = MECA_CRIMP_RIGHT_SERVO_DISABLE;
  servo_vertical_set_point = MECA_CRIMP_VERTICAL_SERVO_DISABLE;
  meca_crimp_update_servo();
}

int meca_crimp_console_handler(const char * command)
{
  if(strcmp(command,"meca_crimp_left_servo") == 0)
  {
    int set;
    if(cocobot_console_get_iargument(0, &set))
    {
      servo_left_set_point = set;
      meca_crimp_update_servo();
    }
    cocobot_console_send_answer("%u", servo_left_set_point);
    return 1;
{'fraction_non_alphanumeric': 0.07500264466306993, 'fraction_numerical': 0.02316724849254205, 'mean_word_length': 5.422554347826087, 'pattern_counts': {'":': 0, '<': 17, '<?xml version=': 0, '>': 23, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '6383103', 'n_tokens_mistral': 4032, 'n_tokens_neox': 3836, 'n_words': 485}