Skip to content
Snippets Groups Projects
Commit 037021f2 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

First working version of the new firmware with a PID.

parent 799e458d
No related branches found
No related tags found
No related merge requests found
Showing
with 484 additions and 94 deletions
...@@ -2,7 +2,7 @@ PROJECT=ax12_fw ...@@ -2,7 +2,7 @@ PROJECT=ax12_fw
######################################################## ########################################################
# afegir tots els fitxers que s'han de compilar aquí # afegir tots els fitxers que s'han de compilar aquí
######################################################## ########################################################
SOURCES=src/main.c src/gpio.c src/dyn_slave.c src/mem.c SOURCES=src/main.c src/gpio.c src/dyn_slave.c src/mem.c src/motor_control.c src/adc.c src/time_base.c src/sensors.c src/comm.c
OBJS=$(SOURCES:.c=.o) OBJS=$(SOURCES:.c=.o)
SRC_DIR=./src/ SRC_DIR=./src/
INCLUDE_DIR=./include/ INCLUDE_DIR=./include/
...@@ -14,7 +14,7 @@ MMCU=atmega8 ...@@ -14,7 +14,7 @@ MMCU=atmega8
CFLAGS=-mmcu=$(MMCU) -Wall -Os -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes CFLAGS=-mmcu=$(MMCU) -Wall -Os -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
LDFLAGS=-mmcu=$(MMCU) -Wl,-Map=$(PROJECT).map -DF_CPU=16000000UL LDFLAGS=-mmcu=$(MMCU) -Wl,-Map=$(PROJECT).map,--section-start,.eeprom=810000 -DF_CPU=16000000UL
HEX_FLASH_FLAGS = -R .eeprom -R .fuse -R .lock -R .signature HEX_FLASH_FLAGS = -R .eeprom -R .fuse -R .lock -R .signature
......
#ifndef _ADC_H
#define _ADC_H
#include <avr/interrupt.h>
#include <avr/io.h>
#define ADC_pote_Port 0 // Input port for potenciometer
#define ADC_temp_Port 2 // Input port for temperature
#define ADC_voltage_Port 3 // Input port for voltage
void adc_init(void);
void adc_start(uint8_t port);
uint8_t adc_is_done(void);
uint16_t adc_get_value(void);
#endif
#ifndef _COMM_H
#define _COMM_H
#include <avr/interrupt.h>
#include <avr/io.h>
void comm_init(void);
void comm_loop(void);
void comm_set_error(uint8_t error);
void comm_clear_error(void);
#endif
#ifndef DYN_SLAVE_H #ifndef DYN_SLAVE_H
#define DYN_SLAVE_H #define DYN_SLAVE_H
#include <avr/interrupt.h>
#include <avr/io.h>
// Instruction identifiers // Instruction identifiers
#define INST_PING 0x01 #define INST_PING 0x01
#define INST_READ 0x02 #define INST_READ 0x02
......
...@@ -26,10 +26,10 @@ ...@@ -26,10 +26,10 @@
// Register Id RAM - declared in dynamixel // Register Id RAM - declared in dynamixel
#define Torque_Enable 0X18 //24 - 0x00 R/W - Torque On/Off #define Torque_Enable 0X18 //24 - 0x00 R/W - Torque On/Off
#define LED 0X19 //25 - 0x00 R/W - LED On/Off #define LED 0X19 //25 - 0x00 R/W - LED On/Off
#define CW_Compliance_Margin 0X1A //26 - 0X01 R/W - CW Compliance margin #define KP_L 0X1A //26 - 0X01 R/W - CW Compliance margin
#define CCW_Compliance_Margin 0X1B //27 - 0X01 R/W - CCW Compliance margin #define KP_H 0X1B //27 - 0X01 R/W - CCW Compliance margin
#define CW_Compliance_Slope 0X1C //28 - 0X20 R/W - CW Compliance slope #define KI_L 0X1C //28 - 0X20 R/W - CW Compliance slope
#define CCW_Compliance_Slope 0X1D //29 - 0X20 R/W - CCW Compliance Slope #define KI_H 0X1D //29 - 0X20 R/W - CCW Compliance Slope
#define Goal_Position_L 0X1E //30 - 0x00 R/W - Lowest byte of Goal Position #define Goal_Position_L 0X1E //30 - 0x00 R/W - Lowest byte of Goal Position
#define Goal_Position_H 0X1F //31 - 0x00 R/W - Highest byte of Goal Position #define Goal_Position_H 0X1F //31 - 0x00 R/W - Highest byte of Goal Position
#define Moving_Speed_L 0X20 //32 - 0xF0 R/W - Lowest byte of Moving Speed #define Moving_Speed_L 0X20 //32 - 0xF0 R/W - Lowest byte of Moving Speed
...@@ -48,8 +48,10 @@ ...@@ -48,8 +48,10 @@
//45 no used //45 no used
#define Moving 0X2E //46 - 0X00 R - Means if there is any movement #define Moving 0X2E //46 - 0X00 R - Means if there is any movement
#define Lock 0X2F //47 - 0x00 R/W - Locking EEPROM #define Lock 0X2F //47 - 0x00 R/W - Locking EEPROM
#define Punch_L 0X30 //48 - 0X20 R/W - Lowest byte of Punch #define KD_L 0X30 //48 - 0X20 R/W - Lowest byte of Punch
#define Punch_H 0X31 //49 - 0X00 R/W - Highest byte of Punch #define KD_H 0X31 //49 - 0X00 R/W - Highest byte of Punch
extern unsigned char ram_data[50];
void ram_init(void); void ram_init(void);
uint8_t ram_read(uint8_t address, uint8_t length, uint8_t **data); uint8_t ram_read(uint8_t address, uint8_t length, uint8_t **data);
......
#ifndef _MOTOR_CONTROL_H
#define _MOTOR_CONTROL_H
#include <avr/interrupt.h>
#include <avr/io.h>
void motor_init(void);
void motor_set_pwm(uint16_t cw,uint16_t ccw);
void motor_control_loop(void);
#endif
#ifndef _SENSORS_H
#define _SENSIRS_H
void sensors_init(void);
void sensors_loop(void);
#endif
#ifndef _TIME_BASE_H
#define _TIME_BASE_H
#include <avr/io.h>
void time_base_init(void);
uint8_t time_base_period(void);
#endif
#include "adc.h"
void adc_init(void)
{
ADMUX|=(1 << REFS0); // use AVcc as the reference
ADMUX&=~(1 << ADLAR); // clear for 10 bit resolution
ADCSRA|=(1 << ADPS2)|(1 << ADPS1)|(1 << ADPS0)|(1 << ADEN); // 128 prescale for 8Mhz, enable ADC
}
void adc_start(uint8_t port)
{
ADMUX&=0xF0;
ADMUX|=port;
ADCSRA|=(1 << ADSC); // Start the ADC conversion
}
uint8_t adc_is_done(void)
{
if(ADCSRA & (1 << ADSC)) // waits for the ADC to finish
return 0x00;
else
return 0x01;
}
uint16_t adc_get_value(void)
{
return ADCL+(ADCH << 8); // ADCH is read so ADC can be updated again
}
#include "comm.h"
#include "dyn_slave.h"
#include "mem.h"
#include "gpio.h"
uint8_t dyn_error;
void do_write(uint8_t address,uint8_t length,uint8_t *data)
{
uint8_t i,num=0;
for(i=address;i<address+length;i++)
{
if(i==ID)
dyn_slave_set_id(data[num]);
else if(i==Baud_Rate)
dyn_slave_set_baudrate(data[num]);
else if(i==LED)
{
if(data[num]==0x00) gpio_led_off();
else gpio_led_on();
}
num++;
}
ram_write(address,length,data);
}
void comm_init(void)
{
dyn_error=NO_ERROR;
}
void comm_loop(void)
{
uint8_t *data;
if(dyn_slave_is_packet_ready())
{
if(dyn_slave_check_id())
{
if(dyn_slave_check_checksum())
{
switch(dyn_slave_get_instruction())
{
case INST_PING: dyn_slave_send_status(dyn_error|NO_ERROR,0x00,0x00000000);
break;
case INST_READ: if(ram_read(dyn_slave_get_address(),dyn_slave_get_read_length(),&data))
dyn_slave_send_status(dyn_error|NO_ERROR,dyn_slave_get_read_length(),data);
else
dyn_slave_send_status(dyn_error|INSTRUCTION_ERROR,0x00,0x00000000);
break;
case INST_WRITE: if(ram_write(dyn_slave_get_address(),dyn_slave_get_write_length(),dyn_slave_get_write_data()))
{
do_write(dyn_slave_get_address(),dyn_slave_get_write_length(),dyn_slave_get_write_data());
dyn_slave_send_status(dyn_error|NO_ERROR,0x00,0x00000000);
}
else
dyn_slave_send_status(dyn_error|INSTRUCTION_ERROR,0x00,0x00000000);
break;
default: dyn_slave_send_status(dyn_error|INSTRUCTION_ERROR,0x00,0x00000000);
break;
}
}
else
dyn_slave_send_status(dyn_error|CHECKSUM_ERROR,0x00,0x00000000);
}
}
}
void comm_set_error(uint8_t error)
{
dyn_error|=error;
}
void comm_clear_error(void)
{
dyn_error=NO_ERROR;
}
...@@ -5,7 +5,8 @@ ...@@ -5,7 +5,8 @@
uint8_t dyn_slave_id; uint8_t dyn_slave_id;
uint8_t dyn_slave_data[128]; uint8_t dyn_slave_data[128];
uint8_t dyn_slave_num_bytes; uint8_t dyn_slave_num_rx_bytes;
uint8_t dyn_slave_num_tx_bytes;
uint8_t dyn_slave_packet_ready; uint8_t dyn_slave_packet_ready;
uint8_t dyn_slave_send_done; uint8_t dyn_slave_send_done;
...@@ -23,18 +24,17 @@ inline void dyn_slave_set_tx(void) ...@@ -23,18 +24,17 @@ inline void dyn_slave_set_tx(void)
ISR(USART_TXC_vect) ISR(USART_TXC_vect)
{ {
UDR=dyn_slave_data[dyn_slave_num_bytes]; if(dyn_slave_num_tx_bytes==dyn_slave_data[3]+4)
if(dyn_slave_num_bytes==dyn_slave_data[3]+4)
{ {
dyn_slave_num_bytes=0; dyn_slave_num_rx_bytes=0;
dyn_slave_set_rx(); dyn_slave_set_rx();
dyn_slave_send_done=1; dyn_slave_send_done=1;
UCSRB&=~(1<<TXCIE);// disable tx interrutps
UCSRA|=0x80;// clear any pending interrupt
UCSRB|=(1<<RXCIE);// enable reception interrupts
} }
else else
dyn_slave_num_bytes++; {
UDR=dyn_slave_data[dyn_slave_num_tx_bytes];
dyn_slave_num_tx_bytes++;
}
} }
ISR(USART_RXC_vect) ISR(USART_RXC_vect)
...@@ -42,30 +42,25 @@ ISR(USART_RXC_vect) ...@@ -42,30 +42,25 @@ ISR(USART_RXC_vect)
static uint8_t length; static uint8_t length;
cli();// disable any other interrupt cli();// disable any other interrupt
dyn_slave_data[dyn_slave_num_bytes]=UDR; dyn_slave_data[dyn_slave_num_rx_bytes]=UDR;
switch(dyn_slave_num_bytes) switch(dyn_slave_num_rx_bytes)
{ {
case 0: if(dyn_slave_data[dyn_slave_num_bytes]==0xFF) case 0: if(dyn_slave_data[dyn_slave_num_rx_bytes]==0xFF)
dyn_slave_num_bytes++; dyn_slave_num_rx_bytes++;
break; break;
case 1: if(dyn_slave_data[dyn_slave_num_bytes]==0xFF) case 1: if(dyn_slave_data[dyn_slave_num_rx_bytes]==0xFF)
dyn_slave_num_bytes++; dyn_slave_num_rx_bytes++;
else else
dyn_slave_num_bytes--; dyn_slave_num_rx_bytes--;
break; break;
case 2: dyn_slave_num_bytes++; case 2: dyn_slave_num_rx_bytes++;
break; break;
case 3: length=dyn_slave_data[dyn_slave_num_bytes]+3; case 3: length=dyn_slave_data[dyn_slave_num_rx_bytes]+3;
dyn_slave_num_bytes++; dyn_slave_num_rx_bytes++;
break; break;
default: if(dyn_slave_num_bytes==length) default: if(dyn_slave_num_rx_bytes==length)
{
dyn_slave_num_bytes=0;
dyn_slave_packet_ready=1; dyn_slave_packet_ready=1;
UCSRB&=~(1<<RXCIE);// disable the rx interrupt dyn_slave_num_rx_bytes++;
}
else
dyn_slave_num_bytes++;
break; break;
} }
sei();// enable all interrutps sei();// enable all interrutps
...@@ -83,12 +78,14 @@ void dyn_slave_init(uint8_t baudrate,uint8_t id) ...@@ -83,12 +78,14 @@ void dyn_slave_init(uint8_t baudrate,uint8_t id)
// initialize the rs485 ports // initialize the rs485 ports
UCSRA = (1<<U2X);// double USART transmission speed UCSRA = (1<<U2X);// double USART transmission speed
dyn_slave_set_baudrate(baudrate); dyn_slave_set_baudrate(baudrate);
UCSRB = (1<<RXEN)|(1<<TXEN)|(1<<RXCIE); UCSRA|=0x40;// clear any pending interrupt
UCSRB = (1<<RXEN)|(1<<TXEN)|(1<<RXCIE)|(1<<TXCIE);
UCSRC = (1<<URSEL)|(1<<UCSZ1)|(1<< UCSZ0); // 8 bit data, no parity, 1 stop bit UCSRC = (1<<URSEL)|(1<<UCSZ1)|(1<< UCSZ0); // 8 bit data, no parity, 1 stop bit
dyn_slave_set_id(id); dyn_slave_set_id(id);
// initialize private variables // initialize private variables
dyn_slave_num_bytes=0; dyn_slave_num_tx_bytes=0;
dyn_slave_num_rx_bytes=0;
dyn_slave_packet_ready=0; dyn_slave_packet_ready=0;
dyn_slave_send_done=0; dyn_slave_send_done=0;
} }
...@@ -120,7 +117,10 @@ uint8_t dyn_slave_check_id(void) ...@@ -120,7 +117,10 @@ uint8_t dyn_slave_check_id(void)
if(dyn_slave_id==dyn_slave_data[2]) if(dyn_slave_id==dyn_slave_data[2])
return 0x01; return 0x01;
else else
{
dyn_slave_num_rx_bytes=0;
return 0x00; return 0x00;
}
} }
uint8_t dyn_slave_check_checksum(void) uint8_t dyn_slave_check_checksum(void)
...@@ -169,7 +169,7 @@ void dyn_slave_send_status(uint8_t error,uint8_t length, uint8_t *data) ...@@ -169,7 +169,7 @@ void dyn_slave_send_status(uint8_t error,uint8_t length, uint8_t *data)
dyn_slave_data[2]=dyn_slave_id; dyn_slave_data[2]=dyn_slave_id;
cs+=dyn_slave_id; cs+=dyn_slave_id;
dyn_slave_data[3]=length+2; dyn_slave_data[3]=length+2;
cs+=length; cs+=length+2;
dyn_slave_data[4]=error; dyn_slave_data[4]=error;
cs+=error; cs+=error;
for(i=0;i<length;i++) for(i=0;i<length;i++)
...@@ -181,9 +181,7 @@ void dyn_slave_send_status(uint8_t error,uint8_t length, uint8_t *data) ...@@ -181,9 +181,7 @@ void dyn_slave_send_status(uint8_t error,uint8_t length, uint8_t *data)
// set in tex mode // set in tex mode
dyn_slave_set_tx(); dyn_slave_set_tx();
// start transmission // start transmission
UCSRA|=0x40;// clear any pending interrupt dyn_slave_num_tx_bytes=1;
UCSRB|=(1<<TXCIE);
dyn_slave_num_bytes=1;
UDR=dyn_slave_data[0]; UDR=dyn_slave_data[0];
} }
......
...@@ -9,61 +9,28 @@ ...@@ -9,61 +9,28 @@
#include "gpio.h" #include "gpio.h"
#include "dyn_slave.h" #include "dyn_slave.h"
#include "mem.h" #include "mem.h"
#include "adc.h"
void do_write(uint8_t address,uint8_t length,uint8_t *data) #include "motor_control.h"
{ #include "time_base.h"
uint8_t i,num=0; #include "sensors.h"
#include "comm.h"
// check is EEPROM space
for(i=address;i<address+length;i++)
{
if(i==ID)
dyn_slave_set_id(data[num]);
num++;
}
}
int16_t main(void) int16_t main(void)
{ {
uint8_t *data;
gpio_init(); gpio_init();
dyn_slave_init(34,1);
ram_init(); ram_init();
dyn_slave_init(ram_data[Baud_Rate],ram_data[ID]);
adc_init();
time_base_init();
sensors_init();
motor_init();
comm_init();
sei(); sei();
while (1) while (1)
{ {
if(dyn_slave_is_packet_ready()) comm_loop();
{ sensors_loop();
if(dyn_slave_check_id()) motor_control_loop();
{
if(dyn_slave_check_checksum())
{
switch(dyn_slave_get_instruction())
{
case INST_PING: dyn_slave_send_status(NO_ERROR,0x00,0x00000000);
break;
case INST_READ: if(ram_read(dyn_slave_get_address(),dyn_slave_get_read_length(),&data))
dyn_slave_send_status(NO_ERROR,dyn_slave_get_read_length(),data);
else
dyn_slave_send_status(INSTRUCTION_ERROR,0x00,0x00000000);
break;
case INST_WRITE: if(ram_write(dyn_slave_get_address(),dyn_slave_get_write_length(),dyn_slave_get_write_data()))
{
do_write(dyn_slave_get_address(),dyn_slave_get_write_length(),dyn_slave_get_write_data());
dyn_slave_send_status(NO_ERROR,0x00,0x00000000);
}
else
dyn_slave_send_status(INSTRUCTION_ERROR,0x00,0x00000000);
break;
default: dyn_slave_send_status(INSTRUCTION_ERROR,0x00,0x00000000);
break;
}
}
else
dyn_slave_send_status(CHECKSUM_ERROR,0x00,0x00000000);
}
}
} }
} }
#include "mem.h" #include "mem.h"
#include "gpio.h"
#include <avr/eeprom.h> #include <avr/eeprom.h>
// dynamixel RAM variables // dynamixel RAM variables
unsigned char ram_data[50]; unsigned char ram_data[50];
// Dynamixel EEPROM variables // Dynamixel EEPROM variables
unsigned char EEMEM eeprom_data[19]={0x0C,0x00,0x00,0x01,0x22,0xFA,0x00,0x00,0xFF,0x03,0x00,0x50,0x3C,0xF0,0xFF,0x03,0x02,0x24,0x24}; unsigned char EEMEM eeprom_data[19]={0x1C,0x00,0x00,0x01,0x22,0xFA,0x00,0x00,0xFF,0x03,0x00,0x50,0x3C,0xF0,0xFF,0x03,0x02,0x24,0x24};
void ram_init(void) void ram_init(void)
{ {
uint8_t i; uint8_t i;
for(i=0;i<Alarm_Shutdown;i++) for(i=0;i<Alarm_Shutdown;i++)
ram_data[i]=eeprom_read_byte(&eeprom_data[i]); ram_data[i]=eeprom_read_byte(&eeprom_data[i]);
for(;i<Punch_H;i++) for(;i<=KD_H;i++)
ram_data[i]=0x00; ram_data[i]=0x00;
ram_data[CW_Compliance_Slope]=0x20; ram_data[KP_L]=0x20;
ram_data[CCW_Compliance_Slope]=0x20; ram_data[KP_H]=0x00;
ram_data[Torque_Limit_L]=ram_data[Max_Torque_L]; ram_data[Torque_Limit_L]=ram_data[Max_Torque_L];
ram_data[Torque_Limit_H]=ram_data[Max_Torque_H]; ram_data[Torque_Limit_H]=ram_data[Max_Torque_H];
ram_data[Punch_L]=0x20;
} }
uint8_t ram_read(uint8_t address, uint8_t length, uint8_t **data) uint8_t ram_read(uint8_t address, uint8_t length, uint8_t **data)
{ {
if((address+length)>Punch_H) if((address+length)>KD_H)
return 0x00; return 0x00;
else else
{ {
...@@ -37,7 +37,7 @@ uint8_t ram_write(uint8_t address, uint8_t length, uint8_t *data) ...@@ -37,7 +37,7 @@ uint8_t ram_write(uint8_t address, uint8_t length, uint8_t *data)
{ {
uint8_t i,num=0; uint8_t i,num=0;
if((address+length)>Punch_H) if((address+length)>KD_H)
return 0x00; return 0x00;
else else
{ {
......
#include "motor_control.h"
#include "time_base.h"
#include "comm.h"
#include "mem.h"
#include "dyn_slave.h"
#include "gpio.h"
#define CW_PWM_MOTOR_PIN PB1
#define CCW_PWM_MOTOR_PIN PB2
#define CONTROL_COUNT 10
int16_t start_position;
int16_t previous_position;
int16_t previous_seek;
int16_t seek_delta;
int16_t count;
void motor_init(void)
{
/* configure the control signals */
DDRB|=(1 << CW_PWM_MOTOR_PIN)|(1 << CCW_PWM_MOTOR_PIN);
motor_set_pwm(0x0000,0x0000);
/* configure the PWM */
TCCR1A|=(1 << COM1A1)|(1 << COM1B1)|(1 << WGM11) | (1 << WGM10); // set none-inverting mode PB1 and PB2, Mode fast PWM @ 10 bits
TCCR1B|=(1 << WGM12)|(1 << CS11) | (1 << CS10) ; // set Mode Fast PWM, 10bit, prescaker 64
// initialize internal variables
previous_position=ram_data[Present_Position_L]+(ram_data[Present_Position_H]<<8);
start_position=previous_position;
previous_seek=previous_position;
seek_delta=-1;
count=0;
}
void motor_set_pwm(uint16_t cw,uint16_t ccw)
{
OCR1A = ccw; //PB1 => set PWM for X% duty cycle @ 10bit
OCR1B = cw; //PB2 => set PWM for Y% duty cycle @ 10bit
}
void motor_control_loop(void)
{
static int16_t current_position;
static int16_t current_velocity;
static int16_t seek_position;
static int16_t seek_velocity;
static int16_t minimum_position;
static int16_t maximum_position;
static int16_t p_component;
static int16_t d_component;
static int16_t i_component;
static int16_t torque_limit;
static int16_t deadband=10;
static int32_t pwm_output;
static uint16_t p_gain;
static uint16_t i_gain;
static uint16_t d_gain;
if(time_base_period())
{
// update control loop
// get all necessary variables
current_position=ram_data[Present_Position_L]+(ram_data[Present_Position_H]<<8);
current_velocity = current_position - previous_position;
ram_data[Present_Speed_L]=current_velocity&0xFF;
ram_data[Present_Speed_H]=((current_velocity>>8)&0xFF);
seek_position=ram_data[Goal_Position_L]+(ram_data[Goal_Position_H]<<8);
seek_velocity=ram_data[Moving_Speed_L]+(ram_data[Moving_Speed_H]<<8);
maximum_position=ram_data[CCW_Angle_Limit_L]+(ram_data[CCW_Angle_Limit_H]<<8);
minimum_position=ram_data[CW_Angle_Limit_L]+(ram_data[CW_Angle_Limit_H]<<8);
torque_limit=ram_data[Torque_Limit_L]+(ram_data[Torque_Limit_H]<<8);
p_gain=ram_data[KP_L]+(ram_data[KP_H]<<8);
i_gain=ram_data[KI_L]+(ram_data[KI_H]<<8);
d_gain=ram_data[KD_L]+(ram_data[KD_H]<<8);
/* saturate the target position */
if (seek_position < minimum_position)
{
seek_position = minimum_position;
comm_set_error(ANGLE_LIMIT_ERROR);
}
if (seek_position > maximum_position)
{
seek_position = maximum_position;
comm_set_error(ANGLE_LIMIT_ERROR);
}
/* update the new target */
if(previous_seek != seek_position)
{
previous_seek=seek_position;
seek_delta=current_position;
start_position=current_position;
}
/* generate the intermediate target point */
if(count==CONTROL_COUNT && seek_delta!=seek_position && seek_velocity>0) // Tick is our time constant
{
if(start_position<seek_position)
{
seek_delta+=seek_velocity;
if(seek_delta>=seek_position)
seek_delta=seek_position;
}
else if(start_position>seek_position)
{
seek_delta-=seek_velocity;
if(seek_delta<=seek_position)
seek_delta=seek_position;
}
}
/* compute the error */
p_component = seek_delta - current_position;
if(count==CONTROL_COUNT)
{
i_component += p_component;
if(i_component<-128) // Somewhat arbitrary anti integral wind-up; we're experimenting
i_component=-128;
else if(i_component>128)
i_component=128;
}
d_component = previous_position - current_position;
previous_position = current_position;
/* compute the PWM */
pwm_output = 0;
if ((p_component > deadband) || (p_component < -deadband))
{
// Apply the proportional component of the PWM output.
pwm_output += (int32_t) p_component * (int32_t) p_gain;
// Apply the integral component of the PWM output.
// pwm_output += (int32_t) i_component * (int32_t) i_gain;
// Apply the derivative component of the PWM output.
// pwm_output += (int32_t) d_component * (int32_t) d_gain;
}
else
i_component = 0;
pwm_output>>=8;
if(pwm_output>torque_limit)
pwm_output=torque_limit;
else if(pwm_output<-torque_limit)
pwm_output=-torque_limit;
if(pwm_output<0)
{
pwm_output=-pwm_output;
motor_set_pwm((uint16_t)pwm_output,0x0000);
}
else
motor_set_pwm(0x0000,(uint16_t)pwm_output);
ram_data[Present_Load_L]=pwm_output&0xFF;
ram_data[Present_Load_H]=((pwm_output>>8)&0xFF);
/* update control loop variable */
if(count==CONTROL_COUNT)
count=0;
else
count++;
}
}
#include "sensors.h"
#include "adc.h"
#include "comm.h"
#include "mem.h"
#include "dyn_slave.h"
typedef enum {adc_get_pos=0,adc_get_temp=1,adc_get_volt=2} adc_state;
uint16_t current_position;
adc_state state;
void sensors_set_pos(uint16_t pos)
{
ram_data[Present_Position_L]=pos&0xFF;
ram_data[Present_Position_H]=(pos>>8)&0xFF;
current_position=pos;
}
void sensors_set_temp(uint16_t temp)
{
uint8_t temperature;
temperature=(((uint32_t)temp*50)>>10);
ram_data[Present_Temperature]=temperature;
// check the limits and generate the error
if(temperature>ram_data[Int_Limit_Temperature])
comm_set_error(OVERHEATING_ERROR);
else
comm_set_error(NO_ERROR);
}
void sensors_set_voltage(uint16_t voltage)
{
uint8_t volt;
volt=(((uint32_t)voltage*200)>>10);
ram_data[Present_Voltage]=volt;
// check the limits and generate the error
if(volt>ram_data[High_Limit_Voltage] || volt<ram_data[Low_Limit_Voltage])
comm_set_error(VOLTAGE_ERROR);
else
comm_set_error(NO_ERROR);
}
void sensors_init(void)
{
// start the voltage conversion
adc_start(ADC_pote_Port);
while(!adc_is_done());
sensors_set_pos(adc_get_value());
adc_start(ADC_temp_Port);
state=adc_get_temp;
}
void sensors_loop(void)
{
if(adc_is_done())// the last AD conversion has finished
{
if(state==adc_get_pos)
{
sensors_set_pos(adc_get_value());
adc_start(ADC_temp_Port);
state=adc_get_temp;
}
else if(state==adc_get_temp)
{
sensors_set_temp(adc_get_value());
adc_start(ADC_voltage_Port);
state=adc_get_volt;
}
else
{
sensors_set_voltage(adc_get_value());
adc_start(ADC_pote_Port);
state=adc_get_pos;
}
}
}
#include "time_base.h"
void time_base_init(void)
{
TCCR0|=(1<<CS01)|(1<<CS00); // clk src with prescaler 64
TIMSK=0x00; // disable all interrupts
TCNT0 = 0x00;
}
uint8_t time_base_period(void)
{
if(TIFR & (1<<TOV0))
{
TIFR|=(1<<TOV0);
return 0x01;
}
else
return 0x00;
}
uint8_t time_base_get_counter(void)
{
return TCNT0;
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment