diff --git a/servo_firmware/rx28/Makefile b/servo_firmware/rx28/Makefile
index 8bdf19a7477384333d9c2fd04ea10ab21736832d..23d8a12ec9563b1d0dffa3b6914ed02098719640 100755
--- a/servo_firmware/rx28/Makefile
+++ b/servo_firmware/rx28/Makefile
@@ -2,7 +2,9 @@ PROJECT=rx28_fw
########################################################
# afegir tots els fitxers que s'han de compilar aquÃ
########################################################
-SOURCES=src/main.c src/TXRX_Dynamixel.c src/CTRL_Dynamixel.c src/MEM_Dynamixel.c src/CFG_HW_Dynamixel.c
+#SOURCES=src/main.c src/TXRX_Dynamixel.c src/CTRL_Dynamixel.c src/MEM_Dynamixel.c src/CFG_HW_Dynamixel.c
+#SOURCES=src/main.c src/CTRL_Dynamixel.c src/MEM_Dynamixel.c src/CFG_HW_Dynamixel.c
+SOURCES=src/main.c src/CFG_HW_Dynamixel.c
OBJS=$(SOURCES:.c=.o)
SRC_DIR=./src/
INCLUDE_DIR=./include/
diff --git a/servo_firmware/rx28/include/CFG_HW_Dynamixel.h b/servo_firmware/rx28/include/CFG_HW_Dynamixel.h
index cee708ddc68e992f8e02c5deb912d4140ef682f5..6ecf506437b44075e396ab287c1547a82e43ecc0 100755
--- a/servo_firmware/rx28/include/CFG_HW_Dynamixel.h
+++ b/servo_firmware/rx28/include/CFG_HW_Dynamixel.h
@@ -1,12 +1,22 @@
#ifndef _CFG_HW_DYNAMIXEL_H
#define _CFG_HW_DYNAMIXEL_H
+#define NULL (void *)0x00000000
+#define FALSE 0
+#define TRUE 1
+
+
#define LED_PIN PD2
+
#define ENABLE_MOTOR_PIN PD7
#define CW_PWM_MOTOR_PIN PB1
#define CCW_PWM_MOTOR_PIN PB2
+#define RX_TX_ENABLE_PIN PB0
+#define RX_PIN PD0
+#define TX_PIN PD1
+
// this is for debuging tonggle LED i times
// void blinkLedN(uint8_t i);
@@ -15,11 +25,11 @@
#define LedON PORTD &= ~(1<<LED_PIN) // ON LED
#define LedTOGGLE PORTD ^= _BV(LED_PIN) // Toggle LED
-#define SET_CW_PWM_MOTOR(x) OCR1A = (x) //PB1 => set PWM for X% duty cycle @ 10bit
-#define SET_CCW_PWM_MOTOR(x) OCR1B = (x) //PB2 => set PWM for Y% duty cycle @ 10bit
+#define SET_CW_PWM_MOTOR(x) (OCR1A = (x)) //PB1 => set PWM for X% duty cycle @ 10bit
+#define SET_CCW_PWM_MOTOR(x) (OCR1B = (x)) //PB2 => set PWM for Y% duty cycle @ 10bit
-#define ENABLE_MOTOR PORTD &= ~(1<<ENABLE_MOTOR_PIN) // enable motor
-#define DISABLE_MOTOR PORTD |= (1<<ENABLE_MOTOR_PIN) // disable motor
+#define DISABLE_MOTOR PORTD &= ~(1<<ENABLE_MOTOR_PIN) // disable motor
+#define ENABLE_MOTOR PORTD |= (1<<ENABLE_MOTOR_PIN) // enable motor
void Config_Hardware(void);
diff --git a/servo_firmware/rx28/include/TXRX_Dynamixel.h b/servo_firmware/rx28/include/TXRX_Dynamixel.h
index 383e08b62e25d4903dd18eda336bd939cff866a8..28d1e6f081a483938ebefc96f0566e0368051dec 100755
--- a/servo_firmware/rx28/include/TXRX_Dynamixel.h
+++ b/servo_firmware/rx28/include/TXRX_Dynamixel.h
@@ -28,8 +28,8 @@
#define NO_ERROR 0x00
// instructions to configure the data direction on the RS485 interface
-#define SET_RS485_TXD PORTD &= 0xBF,PORTD |= 0x80
-#define SET_RS485_RXD PORTD &= 0x7F,PORTD |= 0x40
+#define SET_RS485_TXD PORTB |= (1<<RX_TX_ENABLE_PIN ) // Set pin to TX
+#define SET_RS485_RXD PORTB &= ~(1<<RX_TX_ENABLE_PIN) // Clear pin to RX
// broadcasting address
#define BROADCASTING_ID 0xFE
diff --git a/servo_firmware/rx28/src/CFG_HW_Dynamixel.c b/servo_firmware/rx28/src/CFG_HW_Dynamixel.c
index 5eda0e922896e78cf5e8314c9f90ed036a45466b..788c82db1ad2261df877c79bd8a4a56fe9d54bff 100755
--- a/servo_firmware/rx28/src/CFG_HW_Dynamixel.c
+++ b/servo_firmware/rx28/src/CFG_HW_Dynamixel.c
@@ -1,91 +1,116 @@
#include <avr/io.h>
-#include <util/delay.h>
+//#include <util/delay.h>
#include <avr/interrupt.h>
#include "CFG_HW_Dynamixel.h"
// LED CONTROL
/* void blinkLedN(uint8_t j) // this is for debuging tonggle i times
-{
- uint8_t i;
-
- LedOFF; // led OFF
- _delay_ms(500);
- for (i=1; i<=(2*j); i++) {
- LedTOGGLE; // tonggle led
- _delay_ms(200);
- }
- LedOFF; // led OFF
- _delay_ms(500);
-}*/
-
-
-
-void Config_Hardware(void)
-{
- DDRD |= (1 << LED_PIN); // Set LED as output
- LedON; // ON the LED
- DDRB |= (1 << CW_PWM_MOTOR_PIN); // Set CW pin as output
- SET_CW_PWM_MOTOR(0x0000); //PB1 => set PWM for 0% duty cycle @ 10bit
- DDRB |= (1 << CCW_PWM_MOTOR_PIN); // Set CCW pin as output
- SET_CCW_PWM_MOTOR(0x0000); //PB2 => set PWM for 0% duty cycle @ 10bit
- DDRD |= (1 << ENABLE_MOTOR_PIN); // Set Enable pin as output
- DISABLE_MOTOR; // disable motor
-
-
- cli(); // just to make sure
-
- //*****************************************
- // ini timer/counter0
- /*
- 7 bit 6 bit 5 bit 4 bit 3 bit 2 bit 1 bit 0 bit
- TCCR0 FOC0 - - - - CS02 CS01 CS00
- Timer/Counter Control Register 0
-
-
- CS02 CS01 CS00 DESCRIPTION
- 0 0 0 Timer/Counter0 Disabled
- 0 0 1 No Prescaling
- 0 1 0 Clock / 8
- 0 1 1 Clock / 64
- 1 0 0 Clock / 256
- 1 0 1 Clock / 1024*/
-
- // reloj 20 mhz / 4 instruciones 5000000
- // 5000000/256/prescaler
- // no pre => interrupt => 0.0512 ms
- // 8 => interrupt => 0.4096 ms
- // 64 => interrupt => 3.2768 ms
- // 256 => interrupt => 13.1072 ms
- // 1024 => interrupt => 52.4288 ms
-
- //TCCR0 |= (1<<CS00) | (1<<CS01); // clk src with prescaler 64
- TCCR0 |= (1<<CS02) ; // clk src with prescaler 256
- //TCCR0 |= (1<<CS00) ; // clk src with no prescaler
- TIMSK |= ( 1 << TOIE0); // Enable interrupt timer 0
-
- /* timer/counter0 limitations.. no CTC mode.
- * so move starting point to where neded
- * to do is add X */
- // TCNT0 = 0x9C; // Timer/Counter Register 156 => 0.1 ms counter value set
-
- //*****************************************
- // ini timer/counter1
- TCCR1A |= (1 << COM1A1); // set none-inverting mode PB1
- TCCR1A |= (1 << COM1B1); // set none-inverting mode PB2
- // set Mode Fast PWM, 10bit
- TCCR1A |= (1 << WGM11) | (1 << WGM10); // set Mode Fast PWM, 10bit
- TCCR1B |= (1 << WGM12) ; // set Mode Fast PWM, 10bit
-
- /* set prescaler to and starts PWM
-CS12 CS11 CS10 DESCRIPTION
-0 0 0 Timer/Counter2 Disabled
-0 0 1 No Prescaling
-0 1 0 Clock / 8
-0 1 1 Clock / 64
-1 0 0 Clock / 256
-1 0 1 Clock / 1024*/
- TCCR1B |= (1 << CS11) | (1 << CS10);
-
-
- sei(); // enable all interrupts
+ {
+ uint8_t i;
+
+ LedOFF; // led OFF
+ _delay_ms(500);
+ for (i=1; i<=(2*j); i++) {
+ LedTOGGLE; // tonggle led
+ _delay_ms(200);
+ }
+ LedOFF; // led OFF
+ _delay_ms(500);
+ }*/
+
+void Config_Hardware(void) {
+ DDRD |= (1 << LED_PIN); // Set LED as output
+ LedON; // ON the LED
+ DDRB |= (1 << CW_PWM_MOTOR_PIN); // Set CW pin as output
+ SET_CW_PWM_MOTOR(0x0000); //PB1 => set PWM for 0% duty cycle @ 10bit
+ DDRB |= (1 << CCW_PWM_MOTOR_PIN); // Set CCW pin as output
+ SET_CCW_PWM_MOTOR(0x0000); //PB2 => set PWM for 0% duty cycle @ 10bit
+ DDRD |= (1 << ENABLE_MOTOR_PIN); // Set Enable pin as output
+ DISABLE_MOTOR; // disable motor
+
+ cli(); // just to make sure
+
+/* *************** ini timer/counter0 manage Control cycle interrupt *************************
+
+ 7 bit 6 bit 5 bit 4 bit 3 bit 2 bit 1 bit 0 bit
+ TCCR0 FOC0 - - - CS02 CS01 CS00
+ Timer/Counter Control Register 0
+
+ CS02 CS01 CS00 DESCRIPTION
+ 0 0 0 Timer/Counter0 Disabled
+ 0 0 1 No Prescaling
+ 0 1 0 Clock / 8
+ 0 1 1 Clock / 64
+ 1 0 0 Clock / 256
+ 1 0 1 Clock / 1024 */
+
+ /*TCCR0 |= (1 << CS02)| (0<<CS01)| (0<<CS00); // clk src with prescaler 256
+
+ TIMSK |= (1 << TOIE0); // Enable interrupt timer 0*/
+
+ /* timer/counter0 limitations.. no CTC mode.
+ * so move starting point to where needed
+ * to do is add X */
+ // TCNT0 = 0x9C; // Timer/Counter Register 156 => 0.1 ms counter value set
+
+
+//****************ini timer/counter1* manage PWM ************************
+ /* OC1A = PB1 PIN OC1AB = PB2 PIN
+
+ /* TCCR1A:
+ Bit 7 --- --- --- --- --- --- Bit 0
+ COM1A1 COM1A0 COM1B1 COM1B0 FOC1A FOC1B PWM11 PWM10
+ COM1A1 COM1A0 COM1B1 COM1B0 FOC1A FOC1B WGM11 WGM10
+
+ COM1A1 COM1A0 Compare Output Mode
+ COM1B1 COM1B0
+ 0 0 Disconnect Pin OC1 A/B from Timer/Counter 1
+ 0 1 Toggle OC1 A/B on compare match
+ 1 0 Clear OC1 A/B on compare match
+ 1 1 Set OC1 A/B on compare match */
+
+ TCCR1A |= ( 0 << COM1A0 ) | (1 << COM1A1); // set none-inverting mode PB1
+ TCCR1A |= ( 0 << COM1B0 ) | (1 << COM1B1); // set none-inverting mode PB2
+
+ /*PWM11 PWM10 PWM Mode
+ WGM11 WGM10
+ 0 0 PWM operation disabled
+ 0 1 Timer/Counter 1 is an 8-bit PWM
+ 1 0 Timer/Counter 1 is a 9-bit PWM
+ 1 1 Timer/Counter 1 is a 10-bit PWM*/
+
+ TCCR1A |= (1 << WGM11) | (1 << WGM10); // set Mode Fast PWM, 10bit (PWMxx obsolete use WGMxx)
+
+ /* FOC1A FOC1B
+ * TCCR1B:
+ * Bit 7 --- --- --- --- --- --- Bit 0
+ * ICNC1 ICES1 --- --- CTC1 CS12 CS11 CS10
+ * ICNC1 ICES1 --- WGM13 WGM12 CS12 CS11 CS10
+
+ set prescaler and enables Timer/Counter 1
+ CS12 CS11 CS10 Mode Description
+ 0 0 0 Stop Timer/Counter 1
+ 0 0 1 No Prescaler (Timer Clock = System Clock)
+ 0 1 0 divide clock by 8
+ 0 1 1 divide clock by 64
+ 1 0 0 divide clock by 256
+ 1 0 1 divide clock by 1024
+ 1 1 0 increment timer 1 on T1 Pin falling edge
+ 1 1 1 increment timer 1 on T1 Pin rising edge
+ */
+ TCCR1B |= (0 << CS12) |(0 << CS11) | (1 << CS10); // set No Prescaler
+
+ /* CTC1: Clear Timer/Counter 1 on Compare Match; */
+ TCCR1B |= (0 << WGM13) |(1 << WGM12); // set Mode Fast PWM, 10bit (CTC1 obsolete use WGM12)
+
+ /* ICNC1: Input Capture Noise Canceler; If set, the
+ * Noise Canceler on the ICP pin is activated.
+ * It will trigger the input capture after 4 equal samples.
+ * The edge to be triggered on is selected by the ICES1 bit.*/
+
+ /* ICES1: Input Capture Edge Select;*/
+
+ //TIMSK |= (1 << TOIE1); //enable timer 1 interrupt*/
+
+ sei(); // enable all interrupts
}
diff --git a/servo_firmware/rx28/src/CTRL_Dynamixel.c b/servo_firmware/rx28/src/CTRL_Dynamixel.c
index 0aa66ad5ee51dbad0c4f1a2de37da45a494f8f33..17499e7775e3a25fb4f99d768b4cf4760c5430e9 100755
--- a/servo_firmware/rx28/src/CTRL_Dynamixel.c
+++ b/servo_firmware/rx28/src/CTRL_Dynamixel.c
@@ -2,6 +2,7 @@
#include <avr/io.h>
#include "MEM_Dynamixel.h"
#include "CTRL_Dynamixel.h"
+#include "CFG_HW_Dynamixel.h"
#define INT16_MAX 0x7fff
@@ -35,26 +36,25 @@ void Ini_Position(void)
Write_byte_Dynamixel_RAM(Torque_Enable,1);
}
-int16_t Read_Sensor(uint8_t port)
-{
-
- int16_t ADCval;
+int16_t Read_Sensor(uint8_t port) {
- ADMUX = port; // use #port ADC
- ADMUX |= (1 << REFS0); // use AVcc as the reference
- ADMUX &= ~(1 << ADLAR); // clear for 10 bit resolution
-
- ADCSRA |= (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0); // 128 prescale for 8Mhz
- ADCSRA |= (1 << ADEN); // Enable the ADC
+ int16_t ADCval;
+
+ ADMUX = port; // use #port ADC
+ ADMUX |= (1 << REFS0); // use AVcc as the reference
+ ADMUX &= ~(1 << ADLAR); // clear for 10 bit resolution
+
+ ADCSRA |= (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0); // 128 prescale for 8Mhz
+ ADCSRA |= (1 << ADEN); // Enable the ADC
- ADCSRA |= (1 << ADSC); // Start the ADC conversion
+ ADCSRA |= (1 << ADSC); // Start the ADC conversion
- while(ADCSRA & (1 << ADSC)); // waits for the ADC to finish
+ while (ADCSRA & (1 << ADSC)); // waits for the ADC to finish
- ADCval = ADCL;
- ADCval = (ADCH << 8) + ADCval; // ADCH is read so ADC can be updated again
+ ADCval = ADCL;
+ ADCval = (ADCH << 8) + ADCval; // ADCH is read so ADC can be updated again
- return ADCval;
+ return ADCval;
}
@@ -214,16 +214,17 @@ void Write_Actuator(void)
// place PWM value on respective pin
- if (PWM_Value>CTRL_ZERO){
- if (MotorDir==CTRL_Dir_CCW){
- OCR1B = CTRL_ZERO; //PB2 => set PWM for Y% duty cycle
- OCR1A = PWM_Value; //PB1 => set PWM for X% duty cycle
- }else{
- OCR1A = CTRL_ZERO; //PB1 => set PWM for X% duty cycle
- OCR1B = PWM_Value; //PB2 => set PWM for Y% duty cycle
- }
- } else {
- OCR1A = CTRL_ZERO; //PB1 => set PWM for X% duty cycle
- OCR1B = CTRL_ZERO; //PB2 => set PWM for Y% duty cycle
- }
-}
+ if (PWM_Value > CTRL_ZERO) {
+ if (MotorDir == CTRL_Dir_CCW) {
+ SET_CW_PWM_MOTOR(CTRL_ZERO); //PB1 => set PWM for X% duty cycle
+ SET_CCW_PWM_MOTOR(PWM_Value); //PB2 => set PWM for Y% duty cycle
+ } else {
+ SET_CCW_PWM_MOTOR(CTRL_ZERO); //PB2 => set PWM for Y% duty cycle
+ SET_CW_PWM_MOTOR(PWM_Value); //PB1 => set PWM for X% duty cycle
+ }
+ } else {
+ SET_CW_PWM_MOTOR(CTRL_ZERO); //PB1 => set PWM for X% duty cycle
+ SET_CCW_PWM_MOTOR(CTRL_ZERO); //PB2 => set PWM for Y% duty cycle
+ }
+}
+
diff --git a/servo_firmware/rx28/src/TXRX_Dynamixel.c b/servo_firmware/rx28/src/TXRX_Dynamixel.c
index 56a5a58591633cedf1b6df51d79a614dcbd98158..5cb2f63dc9521802002715b9bcdd9aa041dd0d3d 100755
--- a/servo_firmware/rx28/src/TXRX_Dynamixel.c
+++ b/servo_firmware/rx28/src/TXRX_Dynamixel.c
@@ -1,6 +1,7 @@
#include <avr/interrupt.h>
#include <avr/io.h>
#include <util/delay.h>
+#include "CFG_HW_Dynamixel.h"
#include "TXRX_Dynamixel.h"
#include "MEM_Dynamixel.h"
@@ -62,9 +63,10 @@ unsigned char RS485_receive(unsigned char *data)
void init_RS485(void)
{
// configure the IO ports
- // DDRD - Port D Data Direction Register
- DDRD=DDRD|0xC2;// RX_en, TX_en, TX are outputs 0xC2=11000010
- DDRD=DDRD&0xFE;// RX is an input 0xFE=11111110
+ // Data Direction Register
+ DDRB |= (1 << RX_TX_ENABLE_PIN); // Set (PB0=0) RX_en (PB0=1) TX_en is an output
+ DDRD |= (1 << TX_PIN); //(PD1) TX is an output
+ DDRD &= ~(1 << RX_PIN); //(PD0) RX is an input
// configure the ports to receive data
SET_RS485_RXD;
diff --git a/servo_firmware/rx28/src/main.c b/servo_firmware/rx28/src/main.c
index 51fc747666db09c662bf93ada42fff2a4b75c0e1..7426e68013a1b26fea93c46fa6e8d09191a024b1 100755
--- a/servo_firmware/rx28/src/main.c
+++ b/servo_firmware/rx28/src/main.c
@@ -1,257 +1,268 @@
#include <avr/io.h>
#include <util/delay.h>
-#include <avr/interrupt.h>
-#include "TXRX_Dynamixel.h"
-#include "CTRL_Dynamixel.h"
-#include "MEM_Dynamixel.h"
+//#include <avr/interrupt.h>
+//#include <avr/eeprom.h>
#include "CFG_HW_Dynamixel.h"
-#include <avr/eeprom.h>
-
-#define NULL (void *)0x00000000
-#define FALSE 0
-#define TRUE 1
+//#include "TXRX_Dynamixel.h"
+//#include "CTRL_Dynamixel.h"
+//#include "MEM_Dynamixel.h"
//*************CTRL = INTERRUPT FUNCTION *********************************
uint16_t count = 0;
uint16_t count2 = 0;
/*ISR( TIMER0_OVF_vect) {
- int16_t TorqueEnable;
+ int16_t TorqueEnable;
- sei();
- TorqueEnable = Read_byte_Dynamixel_RAM(Torque_Enable);
- if (TorqueEnable == 1) {
- //if (1){
- //cli(); // disable all interrupts just to make sure
- //TIMSK &= ~( 1 << TOIE0); // disable timer0
- count++;
- if (count == 2) {
- // 1.3s passed
- // LedTONGGLE();
- Control_Cycle();
- HW_Security();
- Write_Actuator();
- count = 0;
- }
- //TIMSK |= ( 1 << TOIE0); // enable timer0
- //sei(); // enable all interrupts
- } else {
- OCR1A = CTRL_ZERO; //PB1 => set PWM for X% duty cycle
- OCR1B = CTRL_ZERO; //PB2 => set PWM for Y% duty cycle
- }
-}*/
+ sei();
+ TorqueEnable = Read_byte_Dynamixel_RAM(Torque_Enable);
+ if (TorqueEnable == 1) {
+ //if (1){
+ //cli(); // disable all interrupts just to make sure
+ //TIMSK &= ~( 1 << TOIE0); // disable timer0
+ count++;
+ if (count == 2) {
+ // 1.3s passed
+ // LedTONGGLE();
+ Control_Cycle();
+ HW_Security();
+ Write_Actuator();
+ count = 0;
+ }
+ //TIMSK |= ( 1 << TOIE0); // enable timer0
+ //sei(); // enable all interrupts
+ } else {
+ OCR1A = CTRL_ZERO; //PB1 => set PWM for X% duty cycle
+ OCR1B = CTRL_ZERO; //PB2 => set PWM for Y% duty cycle
+ }
+ }*/
//**************** M A I N *********************************
int16_t main(void) {
- unsigned char data[128], id, length, instruction, answer[2], status,
- en_vector, i;
+ /*unsigned char data[128], id, length, instruction, answer[2], status,
+ en_vector, i, j;*/
+ uint16_t ii, jj;
+ //double pwm;
// list of read only registers - to exclude from write
/*unsigned char read_only_vector[30] = { 0, 1, 2, 36, 37, 38, 39, 40, 41, 42,
- 43, 44, 45, 46, 67, 68, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 83,
- 84, 85, 86 };*/
+ 43, 44, 45, 46, 67, 68, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 83,
+ 84, 85, 86 };*/
// ini eeprom if first time after run reflash system
/*if (eeprom_read_byte((uint8_t*) Gate_Restore_Eeprom) != 0xCC) {
- Restore_Eeprom_Factory_Values(); //once this procedure is held, no more initialization is performed
- //blinkLedN(3);
- }*/
+ Restore_Eeprom_Factory_Values(); //once this procedure is held, no more initialization is performed
+ //blinkLedN(3);
+ }*/
//------EEPROM initial values-------------
/*Restore_EepromVAR();
- //blinkLedN(5);
- // get value of Motor ID
- rs485_address = eepromVAR[ID];*/
+ //blinkLedN(5);
+ // get value of Motor ID
+ rs485_address = eepromVAR[ID];*/
// configure AVR chip i/o
Config_Hardware();
/*// asign actual position to goal position and asume zero motor turns
- Ini_Position();
+ Ini_Position();
- // initialize the RS485 interface
- init_RS485();*/
+ // initialize the RS485 interface
+ init_RS485();*/
// end of inicialization
LedOFF;
-
+ ENABLE_MOTOR;
+ //DISABLE_MOTOR;
+ ii = 900;
+ jj = 0;
+ OCR1A = 0;
while (1) {
- //LedOFF;
+ //LedTOGGLE; // funciona
+ //LedON; // funciona
+ //LedOFF; // funciona
+ _delay_ms(200); // funciona
+ ii = ii + 10;
+ SET_CCW_PWM_MOTOR(ii); // max 0x3ff o 1023 // funciona
+ //SET_CW_PWM_MOTOR(i); // max 0x3ff o 1023 // funciona
+ if (ii > 1020) {ii = 100;}
+ LedTOGGLE; // funciona
+
/*status = RxRS485Packet(&id, &instruction, &length, data);
- if (status == CHECK_ERROR) {
- TxRS485Packet(rs485_address, CHECKSUM_ERROR, 0, NULL);
- } else if (status == CORRECT) {
- //LedON;
- if (id == rs485_address) {
- switch (instruction) {
- //***********************************************************
- // P I N G
- case INST_PING:
- TxRS485Packet(rs485_address, NO_ERROR, 0, NULL);
- break;
- //*********************************************************
- // R E A D and send info to pc
- case INST_READ:
- if (data[0] <= 18) { // if 0 <= IDinstruction <= 18 caso eeprom
- if (data[0] >= 0) {
- switch (data[1]) // tamaño del paquete que pide
- {
- case 1: // case BYTE
- TxRx_Read_byte_Dynamixel_EEPROM(data[0],
- answer);
- break; //................................................
- case 2: // case WORD
- TxRx_Read_word_Dynamixel_EEPROM(data[0],
- answer);
- break; //................................................
- default:
- TxRS485Packet(rs485_address, INSTRUCTION_ERROR,
- 0, NULL);
- break; //................................................
- }
- } else {
- TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0,
- NULL);
- } // else id error
- } else if (data[0] <= 86) { // else if 24 <= ID <= 86 ram
- if (data[0] >= 24) {
- switch (data[1]) { // tamaño del paquete que pide
- case 1: // case BYTE
- TxRx_Read_byte_Dynamixel_RAM(data[0], answer);
- break; //................................................
- case 2: // case WORD
+ if (status == CHECK_ERROR) {
+ TxRS485Packet(rs485_address, CHECKSUM_ERROR, 0, NULL);
+ } else if (status == CORRECT) {
+ //LedON;
+ if (id == rs485_address) {
+ switch (instruction) {
+ //***********************************************************
+ // P I N G
+ case INST_PING:
+ TxRS485Packet(rs485_address, NO_ERROR, 0, NULL);
+ break;
+ //*********************************************************
+ // R E A D and send info to pc
+ case INST_READ:
+ if (data[0] <= 18) { // if 0 <= IDinstruction <= 18 caso eeprom
+ if (data[0] >= 0) {
+ switch (data[1]) // tamaño del paquete que pide
+ {
+ case 1: // case BYTE
+ TxRx_Read_byte_Dynamixel_EEPROM(data[0],
+ answer);
+ break; //................................................
+ case 2: // case WORD
+ TxRx_Read_word_Dynamixel_EEPROM(data[0],
+ answer);
+ break; //................................................
+ default:
+ TxRS485Packet(rs485_address, INSTRUCTION_ERROR,
+ 0, NULL);
+ break; //................................................
+ }
+ } else {
+ TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0,
+ NULL);
+ } // else id error
+ } else if (data[0] <= 86) { // else if 24 <= ID <= 86 ram
+ if (data[0] >= 24) {
+ switch (data[1]) { // tamaño del paquete que pide
+ case 1: // case BYTE
+ TxRx_Read_byte_Dynamixel_RAM(data[0], answer);
+ break; //................................................
+ case 2: // case WORD
- //LedTOGGLE;
- TxRx_Read_word_Dynamixel_RAM(data[0], answer);
- break; //................................................
- default:
- TxRS485Packet(rs485_address, INSTRUCTION_ERROR,
- 0, NULL);
- break; //................................................
- }
- } else {
- TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0,
- NULL);
- } // else id error
- } else if (data[0] == 87) { // else if ID = 87 ram
- switch (data[1]) { // tamaño del paquete que pide
- case 1: // case BYTE
- TxRx_Read_byte_Dynamixel_RAM(data[0], answer);
- break; //................................................
- default:
- TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0,
- NULL);
- break; //................................................
- }
- } else {
- TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0,
- NULL);
- } // else id error
- break;
+ //LedTOGGLE;
+ TxRx_Read_word_Dynamixel_RAM(data[0], answer);
+ break; //................................................
+ default:
+ TxRS485Packet(rs485_address, INSTRUCTION_ERROR,
+ 0, NULL);
+ break; //................................................
+ }
+ } else {
+ TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0,
+ NULL);
+ } // else id error
+ } else if (data[0] == 87) { // else if ID = 87 ram
+ switch (data[1]) { // tamaño del paquete que pide
+ case 1: // case BYTE
+ TxRx_Read_byte_Dynamixel_RAM(data[0], answer);
+ break; //................................................
+ default:
+ TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0,
+ NULL);
+ break; //................................................
+ }
+ } else {
+ TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0,
+ NULL);
+ } // else id error
+ break;
- //**********************************************************
- // W R I T E info send by pc
- case INST_WRITE:
- en_vector = FALSE; // list of read only registers - to exclude from write
- //unsigned char read_only_vector[30]={0,1,2,36,37,38,39,40,41,42,43,44,45,46,67,68,71,72,73,74,75,76,77,78,79,80,83,84,85,86};
- for (i = 0; i < 30; i++) {
- if (data[0] == read_only_vector[i]) {
- en_vector = TRUE;
- break;
- }
- }
+ //**********************************************************
+ // W R I T E info send by pc
+ case INST_WRITE:
+ en_vector = FALSE; // list of read only registers - to exclude from write
+ //unsigned char read_only_vector[30]={0,1,2,36,37,38,39,40,41,42,43,44,45,46,67,68,71,72,73,74,75,76,77,78,79,80,83,84,85,86};
+ for (i = 0; i < 30; i++) {
+ if (data[0] == read_only_vector[i]) {
+ en_vector = TRUE;
+ break;
+ }
+ }
- if (en_vector == FALSE) {
- if (data[0] <= 18) { // if 0 <= IDinstruction <= 18 caso eeprom
- if (data[0] >= 0) {
- if (Read_byte_Dynamixel_RAM(Lock) == 1) { // is eeprom write lock open
- switch (length) // tamaño del paquete que pide
- {
- case 2: // case BYTE
- TxRx_Write_byte_Dynamixel_EEPROM(
- data[0], data, length);
- break; //................................................
- case 3: // case WORD
- TxRx_Write_word_Dynamixel_EEPROM(
- data[0], data, length);
- break; //................................................
- default:
- TxRS485Packet(rs485_address,
- INSTRUCTION_ERROR, 0, NULL);
- break; //................................................
- }
- switch (data[0]) // aditional instructions for particular registers in eeprom
- {
- case ID: // case EEMPROM and BYTE
- rs485_address = eepromVAR[ID];
- break; //..................................................
- case Baud_Rate: // case EEMPROM and BYTE
- Baud_Rate_Value();
- break; //..................................................
- default:
- ;
- break; //................................................
- }
- Write_byte_Dynamixel_RAM(Lock, 0); // close eeprom write lock
- } else {
- TxRS485Packet(rs485_address,
- INSTRUCTION_ERROR, 0, NULL);
- } // eeprom write lock closed
- } else {
- TxRS485Packet(rs485_address, INSTRUCTION_ERROR,
- 0, NULL);
- } // else id error
- } else if (data[0] <= 86) { // else if 24 <= ID <= 86 ram
- if (data[0] >= 24) {
- switch (length) { // tamaño del paquete que pide
- case 2: // case BYTE
- TxRx_Write_byte_Dynamixel_RAM(data[0], data,
- length);
- break; //................................................
- case 3: // case WORD
- TxRx_Write_word_Dynamixel_RAM(data[0], data,
- length);
- break; //................................................
- default:
- TxRS485Packet(rs485_address,
- INSTRUCTION_ERROR, 0, NULL);
- break; //................................................
- }
- switch (data[0]) { // aditional instructions for particular registers in ram
- case LED: // case RAM and BYTE
- if (Read_byte_Dynamixel_RAM(LED) == 1) {
- LedON;
- } else {
- LedOFF;
- }
- break; //................................................
- default:
- ;
- break; //................................................
- }
- } else {
- TxRS485Packet(rs485_address, INSTRUCTION_ERROR,
- 0, NULL);
- } // else id error
- } else if (data[0] == 87) { // else if ID = 87 ram // case BYTE
- TxRx_Write_byte_Dynamixel_RAM(data[0], data,
- length);
- } else {
- TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0,
- NULL);
- } // else id error
- } else {
- TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0,
- NULL);
- } // else in only read register list
- break;
+ if (en_vector == FALSE) {
+ if (data[0] <= 18) { // if 0 <= IDinstruction <= 18 caso eeprom
+ if (data[0] >= 0) {
+ if (Read_byte_Dynamixel_RAM(Lock) == 1) { // is eeprom write lock open
+ switch (length) // tamaño del paquete que pide
+ {
+ case 2: // case BYTE
+ TxRx_Write_byte_Dynamixel_EEPROM(
+ data[0], data, length);
+ break; //................................................
+ case 3: // case WORD
+ TxRx_Write_word_Dynamixel_EEPROM(
+ data[0], data, length);
+ break; //................................................
+ default:
+ TxRS485Packet(rs485_address,
+ INSTRUCTION_ERROR, 0, NULL);
+ break; //................................................
+ }
+ switch (data[0]) // aditional instructions for particular registers in eeprom
+ {
+ case ID: // case EEMPROM and BYTE
+ rs485_address = eepromVAR[ID];
+ break; //..................................................
+ case Baud_Rate: // case EEMPROM and BYTE
+ Baud_Rate_Value();
+ break; //..................................................
+ default:
+ ;
+ break; //................................................
+ }
+ Write_byte_Dynamixel_RAM(Lock, 0); // close eeprom write lock
+ } else {
+ TxRS485Packet(rs485_address,
+ INSTRUCTION_ERROR, 0, NULL);
+ } // eeprom write lock closed
+ } else {
+ TxRS485Packet(rs485_address, INSTRUCTION_ERROR,
+ 0, NULL);
+ } // else id error
+ } else if (data[0] <= 86) { // else if 24 <= ID <= 86 ram
+ if (data[0] >= 24) {
+ switch (length) { // tamaño del paquete que pide
+ case 2: // case BYTE
+ TxRx_Write_byte_Dynamixel_RAM(data[0], data,
+ length);
+ break; //................................................
+ case 3: // case WORD
+ TxRx_Write_word_Dynamixel_RAM(data[0], data,
+ length);
+ break; //................................................
+ default:
+ TxRS485Packet(rs485_address,
+ INSTRUCTION_ERROR, 0, NULL);
+ break; //................................................
+ }
+ switch (data[0]) { // aditional instructions for particular registers in ram
+ case LED: // case RAM and BYTE
+ if (Read_byte_Dynamixel_RAM(LED) == 1) {
+ LedON;
+ } else {
+ LedOFF;
+ }
+ break; //................................................
+ default:
+ ;
+ break; //................................................
+ }
+ } else {
+ TxRS485Packet(rs485_address, INSTRUCTION_ERROR,
+ 0, NULL);
+ } // else id error
+ } else if (data[0] == 87) { // else if ID = 87 ram // case BYTE
+ TxRx_Write_byte_Dynamixel_RAM(data[0], data,
+ length);
+ } else {
+ TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0,
+ NULL);
+ } // else id error
+ } else {
+ TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0,
+ NULL);
+ } // else in only read register list
+ break;
- //**********************************************************
- //********* D E F A U L T *****************************
- default:
- TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0, NULL);
- break;
- //***************************************************
- }
- }
- }*/
+ //**********************************************************
+ //********* D E F A U L T *****************************
+ default:
+ TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0, NULL);
+ break;
+ //***************************************************
+ }
+ }
+ }*/
}
}