Brazo robótico con simulación en tiempo real con SolidWorks y LabVIEW

#FUSES NOMCLR

#define USB_CONFIG_BUS_POWER 500 //500ma corriente de entrada

#include "usb_cdc.h"
 //Registro de trasnmision y recepcion

int16 tiempos[4];        // Usado para el ciclo actual
int16 tiempos_nuevos[4]; // Buffer temporal
int pines[4];


// Leer ADCs 10 bits y convierte el valor de 500us a 1950us para acho de pulso
void leer_adc() {

   set_adc_channel(0); delay_us(10);
   tiempos_nuevos[0] = ((int32)read_adc() * 1950 / 1023) + 500;

   set_adc_channel(1); delay_us(10);
   tiempos_nuevos[1] = ((int32)read_adc() * 1950 / 1023) + 500;

   set_adc_channel(2); delay_us(10);
   tiempos_nuevos[2] = ((int32)read_adc() * 1950 / 1023) + 500;

   set_adc_channel(3); delay_us(10);
   tiempos_nuevos[3] = ((int32)read_adc() * 1950 / 1023) + 500;
   
      
   if(usb_enumerated()) //esta asignado a un puerto COM?
   {
    printf(usb_cdc_putc,"%04lu,%04lu,%04lu,%04lu\r\n",tiempos_nuevos[0],tiempos_nuevos[1],tiempos_nuevos[2],tiempos_nuevos[3]);
   }
     
}

// Ordenar valores en tiempos[] y pines[]
void ordenar_servos() {

   // Copiar valores desde buffer temporal
   for (int i = 0; i < 4; i++) {
      tiempos[i] = tiempos_nuevos[i];
      pines[i] = i; // pines 0–3
   }

   // Ordenamiento por burbuja simple
   for (int i = 0; i < 3; i++) {
      for (int j = i + 1; j < 4; j++) {
         if (tiempos[i] > tiempos[j]) {
            int16 tmp_tiempo = tiempos[i];
            tiempos[i] = tiempos[j];
            tiempos[j] = tmp_tiempo;

            int tmp_pin = pines[i];
            pines[i] = pines[j];
            pines[j] = tmp_pin;
         }
      }
   }
}

void main() {

   delay_ms(500);//Espera que la fuente se estabilice

   setup_adc(ADC_CLOCK_INTERNAL);
   
   //Habilita el timer0 16 bits
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); //5.461 ms overflow clock 48Mhz
      

   leer_adc();          // Lectura inicial
   ordenar_servos();    // Ordenamiento inicial
   
   usb_init();
   
   while (true) {
   // Encender todas las salidas
      output_bit(PIN_B0,1);
      output_bit(PIN_B1,1);
      output_bit(PIN_B2,1);
      output_bit(PIN_B3,1);
   
      int16 tiempo_espera = 0;
   
   // Apagar cada pin después de generar el ancho de pulso
      for (int i = 0; i < 4; i++) {
         int16 espera = tiempos[i] - tiempo_espera;
         delay_us(espera);
      
         if (pines[i] == 0) output_bit(PIN_B0,0);
         else if (pines[i] == 1) output_bit(PIN_B1,0);
         else if (pines[i] == 2) output_bit(PIN_B2,0);
         else if (pines[i] == 3) output_bit(PIN_B3,0);
      
         tiempo_espera = tiempos[i];
      }
   
   //Resetea el timer y ejecución de las sub-rutinas
      set_timer0(0); // Reset timer;

      int16 tiempoM = tiempos[3];
      leer_adc();    // Leer ADCs
      ordenar_servos(); // Copiar y ordenar datos nuevos al final del ciclo
      
   //Calcula el tiempo pasado del timer0 en microsegundos
      int16 tiempo_pasado = ((int32)get_timer0()*5461)/65535;  //  5461 overflow / 65535 16bits = 1/5; 
      
   // Calcula el tiempo de esperar para completar los 20 ms del periodo
      int16 espera = 20000 - tiempoM - tiempo_pasado; 
   
      if (espera > 0) delay_us((int16)espera);
      
   }
}