Там написано:
Hardware PWM
Hardware PWM is perfect for creating a perfect high frequency squarewave without any cost to the processor. Servos using hardware PWM can only be used on 16 bit pins, while regular DC motors can be used on any PWM pin.
8-bit timer pins: G5 (T0), H6/B4 (T2)
16-bit timer pins: B5/B6/B7 (T1), E3/E4/E5 (T3), H3/H4/H5 (T4), L3/L4/L5 (T5)
Вроде написано, что 12 ног могут отдавать аппаратный ШИМ. Или врет гад?
Добавлено спустя 4 минуты 37 секунд:
Переписал вот прошивку с использованием таймера (еще не компилировал, на рабочем месте экономиста не предусмотрено компиляторов).
- Код: Выделить всё • Развернуть
//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;
}
}
С нетерпением жду критики.
