roboforum.ru

//attempt at using the Axon and Axon II as a servo controller


//define how data should be sent (choose only one)
//#define U0
#define USB

//declare microcontroller
//#define AXON
#define AXON2

#define DEBUG


//WebbotLib Includes
#ifdef AXON
   #include "sys/axon.h"
#endif
#ifdef AXON2
   #include "sys/axon2.h"
#endif

//#include "buffer.h"
#include "iopin.h"      
#include "timer.h"      
#include "rprintf.h"   //use for UART
#include "inttypes.h"


//UART defines (name your UART)
#define U0_UART UART0
#define USB_UART UART1
//UART baud defines (change baud rate)
#define U0_BAUD (BAUD_RATE)230400
#define USB_BAUD (BAUD_RATE)230400
//UART define which uart to use
#define U0_ACTIVATE &uart0SendByte
#define USB_ACTIVATE &uart1SendByte
//getting data
#define GET_DATA_U0 uart0GetByte()
#define GET_DATA_USB uart1GetByte()


//create a big buffer for UART
#ifdef U0
   #define UART0_RX_BUFFER_SIZE 100
#endif
#ifdef USB
   #define UART1_RX_BUFFER_SIZE 100
#endif

#include "hardware.h"   //declare your servo and sensor ports here


int8_t lastByte=-1;         //store last recieved character
//lastByte = -1;
int8_t buff_array[100];   //store last command
uint8_t buff_pos=0;      //record current position in array
uint8_t buff_read=0;   //record current position in array
boolean command_received=0;   //active last command
SERV *serva[32];
SERV *serva2sort[32];

int count_us;
enum command_state commst=empty;

int servo_counter = 0;
int servos_sorted=0;

void save_move(uint8_t *channel, uint16_t *pulsewidth, uint16_t *speed, uint16_t *time)
{
   if (*pulsewidth)
   {
      for(int i=0; i<32; i++)
      {
         if (serva[i]->channel == *channel)
         {
            if (*time)
            {
               long int tbs;
               if (serva[i]->target > serva[i]->posnow)
               {
                  tbs = (serva[i]->target - serva[i]->posnow) / *time * 1000;
               } else {
                  tbs = (serva[i]->posnow - serva[i]->target) / *time * 1000;
               }
               if (tbs < *speed)
               {
                  *speed = tbs;
               }
            }
            serva[i]->target = *pulsewidth;
            serva[i]->speed = *speed;
            rprintf("Parsed command: channel %d, target %d, speed %d,\n", *channel, *pulsewidth, *speed);
         }
      }
      *channel=0;
      *pulsewidth=0;
      *speed=0;
   }
}


//initialize hardware, ie servos, UART, etc.
void appInitHardware(void)
{

   #ifdef AXON2
      led_off();
   #endif
      
   //setup UART0 or USB
   #ifdef USB
      uartInit(USB_UART, USB_BAUD);
      rprintfInit(USB_ACTIVATE);
   #endif
   #ifdef U0
      uartInit(U0_UART, U0_BAUD);//bluetooth
      rprintfInit(U0_ACTIVATE);
   #endif
   
   for (int i; i<32;i++) //init servo data
   {
      //serva[i] = &(SERV);
      serva[i]->channel = i;
      serva[i]->target = 1500;
      serva[i]->posnow = 1500;
      serva[i]->speed = 0;
   }
   
}


TICK_COUNT appInitSoftware(TICK_COUNT loopStart)
   {
   rprintf("init\n");
   count_us = clockGetus();
   return 0;
   }

   
// This is the main loop
TICK_COUNT appControl(LOOP_COUNT loopCount, TICK_COUNT loopStart)
{
   ////////////GET COMMAND/////////////
   #ifdef USB
      lastByte=GET_DATA_USB;
   #endif
   #ifdef U0
      lastByte=GET_DATA_U0;
   #endif

   //fill up array with command recieved from UART
   if(lastByte!=-1)//has new data
   {
      buff_array[buff_pos]=lastByte;
      buff_pos++;

      if(lastByte == 0x0d)//if command is finished
         command_received=1;
   }

   //when command is received, parse it
   if(command_received)
   {
      uint8_t incoming_channel=0;
      uint16_t incoming_pulsewidth=0;
      uint16_t incoming_speed=0;
      uint16_t incoming_time=0;
      commst = empty;
      rprintf("Command received:");
      rprintfMemoryDump(buff_array, 0, buff_pos);
      rprintf("\n");
      //look for 'T' command first
      buff_read=0;
      while(buff_read <= buff_pos)
      {
         if (buff_array[buff_read] == 'T' || buff_array[buff_read] == 't')
         {
            commst = time;
         } else {
            if (commst == time)
            {
               incoming_time = incoming_time * 10 + (buff_array[buff_read] & 0x0F);
            } else {
               commst = empty;
            }
         }
         buff_read++;
      }
      
      //while the full array hasn't been read yet
      //parse the rest of commands
      buff_read=0;
      while(buff_read<=buff_pos)
      {
         if (buff_array[buff_read] > 0x2F && buff_array[buff_read] < 0x3A && commst )
         {
            switch (commst)
            {
               case channel:
                  incoming_channel = incoming_channel * 10 + (buff_array[buff_read] & 0x0F);
                  break;
               case pulsewidth:
                  incoming_pulsewidth = incoming_pulsewidth * 10 + (buff_array[buff_read] & 0x0F);
                  break;
               case speed:
                  incoming_speed = incoming_speed * 10 + (buff_array[buff_read] & 0x0F);
                  break;
               case time: case empty: case positionoffset:
                  break;
            }
         } else {
            switch (buff_array[buff_read])
            {
               case 0x0d:
                  save_move(&incoming_channel, &incoming_pulsewidth, &incoming_speed, &incoming_time);
                  break;
               case '#':
                  save_move(&incoming_channel, &incoming_pulsewidth, &incoming_speed, &incoming_time);
                  commst = channel;
                  break;
               case 'P': case 'p':
                  commst = pulsewidth;
                  break;
               case 'S': case 's':
                  commst = speed;
                  break;
               case 'T': case 't':
                  commst = empty;
                  break;
               default:
                  commst = empty;
            }
         }
         buff_read++;
      }
      buff_pos=0;//reset array
      command_received=0;
   }

   
   // send pulses to servos every 18 ms
   if (clockHasElapsed(count_us, 18000))
   {
      count_us = clockGetus();
      rprintf("Time is: %d\n", count_us);
      
      //define short-term target for each servo
      long int pos = 0;
      for (int i; i<32;i++)
      {
         pos = serva[i]->posnow;
         if (serva[i]->target > pos)
         {
            pos += serva[i]->speed/50;
            if (pos > serva[i]->target) pos = serva[i]->target;
         }  else {
            pos -= serva[i]->speed/50;
            if (pos < serva[i]->target) pos = serva[i]->target;
         }
         serva[i]->posnow = pos;
      }

      memcpy(serva2sort, serva, 32 * sizeof(*serva));
      //sort servos ascending by short-term target
      int isDone=0;
      SERV *tmp;
      for (int i=0;(i<32-1)&&(!isDone);i++)
      {
         isDone=1;
         for (int j=0;j<32-i-1;j++)
         {
            if (serva2sort[j]->posnow > serva2sort[j+1]->posnow)
            {
               // keep number and port in sync with the position so we know which port to change!
               tmp = serva2sort[j];
               serva2sort[j] = serva2sort[j+1];
               serva2sort[j+1] = tmp;

               isDone=0;
            }
         }
      }
      //*tmp = 0;
      memcpy(serva, serva2sort, 32 * sizeof(*serva));
      
      #ifdef DEBUG
      for (int i; i<32;i++)
      {
         rprintf("Servo state: order: %d, channel: %d, target: %d, posnow: %d, speed: %d", i, serva[i]->channel, serva[i]->target, serva[i]->posnow, serva[i]->speed);
      }
      #endif
      ready2run = 1;
   }

   return 0;
}

ISR(TIMER1_CAPT_vect)
{
   if (ready2run)
   {
      // put all servos' pin to high
      if
      for (int i; i<32;i++)
      {
         pin_high(serva[i]->pin);
      }
      OCR1A += serva[0]->posnow;
   } else {
      int offset;
      if (servos_sorted)
      {
         offset = serva[servo_counter+1]->posnow - serva[servo_counter]->posnow;
      }
      pin_low(serva[i]->pin);
      servo_counter =(servo_counter + 1) % 32;
      OCR1A += offset;
      ready2run = 0;
   }
   
}