diff --git a/servo_firmware/rx28/src/CFG_HW_Dynamixel.c b/servo_firmware/rx28/src/CFG_HW_Dynamixel.c
index 788c82db1ad2261df877c79bd8a4a56fe9d54bff..c4a92565843efe53d0b1c4e0d262bca1de6dfd56 100755
--- a/servo_firmware/rx28/src/CFG_HW_Dynamixel.c
+++ b/servo_firmware/rx28/src/CFG_HW_Dynamixel.c
@@ -44,9 +44,9 @@ void Config_Hardware(void) {
1 0 0 Clock / 256
1 0 1 Clock / 1024 */
- /*TCCR0 |= (1 << CS02)| (0<<CS01)| (0<<CS00); // clk src with prescaler 256
+ TCCR0 |= (1 << CS02)| (0<<CS01)| (0<<CS00); // clk src with prescaler 256
- TIMSK |= (1 << TOIE0); // Enable interrupt timer 0*/
+ TIMSK |= (1 << TOIE0); // Enable interrupt timer 0
/* timer/counter0 limitations.. no CTC mode.
* so move starting point to where needed
diff --git a/servo_firmware/rx28/src/main.c b/servo_firmware/rx28/src/main.c
index a8eb6f09386ce369c6e9aefa6e45f651bec08bc1..5a55c45d23eddd5fb0596c70981e1b1c2106a57f 100755
--- a/servo_firmware/rx28/src/main.c
+++ b/servo_firmware/rx28/src/main.c
@@ -1,7 +1,7 @@
#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>
-//#include <avr/eeprom.h>
+#include <avr/eeprom.h>
#include "CFG_HW_Dynamixel.h"
//#include "TXRX_Dynamixel.h"
#include "CTRL_Dynamixel.h"
@@ -18,26 +18,19 @@ ISR( TIMER0_OVF_vect) {
//cli(); // disable all interrupts just to make sure
TorqueEnable = Read_byte_Dynamixel_RAM(Torque_Enable);
if (TorqueEnable == 1) {
- //if (0) {
- //ENABLE_MOTOR;
- DISABLE_MOTOR;
+ ENABLE_MOTOR;
count++;
- count2=Read_Sensor(CTRL_Encoder_Port);
- //if (count == 1) {
- if (count > count2) {
- LedTOGGLE;
- //Control_Cycle();
- //HW_Security();
- //Write_Actuator();
- count = 0;
- }
- if (count == 1023) {
+ if (count == 1) {
+ //LedTOGGLE;
+ Control_Cycle();
+ HW_Security();
+ Write_Actuator();
count = 0;
}
} else {
DISABLE_MOTOR;
- SET_CCW_PWM_MOTOR(0); //PB1 => set PWM for X% duty cycle max 0x3ff o 1023 // funciona
- SET_CW_PWM_MOTOR(0); //PB2 => set PWM for Y% duty cycle max 0x3ff o 1023 // funciona
+ SET_CW_PWM_MOTOR(0); //PB1 => set PWM for X% duty cycle max 0x3ff o 1023 // funciona
+ SET_CCW_PWM_MOTOR(0); //PB2 => set PWM for Y% duty cycle max 0x3ff o 1023 // funciona
//LedTOGGLE;
}
}
@@ -46,22 +39,21 @@ ISR( TIMER0_OVF_vect) {
int16_t main(void) {
unsigned char data[128], id, length, instruction, answer[2], status,
en_vector, i, j;
+ uint16_t ii, jj;
// 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 };*/
- // 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);
- }*/
+ // 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
+ }
- //------EEPROM initial values-------------
- /*Restore_EepromVAR();
- //blinkLedN(5);
- // get value of Motor ID
- rs485_address = eepromVAR[ID];*/
+ //------EEPROM initial values-------------
+ Restore_EepromVAR();
+ // get value of Motor ID
+ //rs485_address = eepromVAR[ID];
// configure AVR chip i/o
Config_Hardware();
@@ -72,8 +64,20 @@ int16_t main(void) {
// end of inicialization
LedOFF;
+
+ ii = 0;
+ jj=1;
while (1) {
+ Write_word_Dynamixel_RAM(Goal_Position_L,ii);
+ _delay_ms(500); // funciona
+ //ii=500;
+ ii = ii + 100*jj;
+ LedTOGGLE;
+ if (ii == 900) {jj=-1;}
+ if (ii == 100) {jj=1;}
+
+
/*status = RxRS485Packet(&id, &instruction, &length, data);
if (status == CHECK_ERROR) {
TxRS485Packet(rs485_address, CHECKSUM_ERROR, 0, NULL);