diff --git a/servo_firmware/rx28/src/main.c b/servo_firmware/rx28/src/main.c index 5a55c45d23eddd5fb0596c70981e1b1c2106a57f..a8eb6f09386ce369c6e9aefa6e45f651bec08bc1 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,19 +18,26 @@ ISR( TIMER0_OVF_vect) { //cli(); // disable all interrupts just to make sure TorqueEnable = Read_byte_Dynamixel_RAM(Torque_Enable); if (TorqueEnable == 1) { - ENABLE_MOTOR; + //if (0) { + //ENABLE_MOTOR; + DISABLE_MOTOR; count++; - if (count == 1) { - //LedTOGGLE; - Control_Cycle(); - HW_Security(); - Write_Actuator(); + count2=Read_Sensor(CTRL_Encoder_Port); + //if (count == 1) { + if (count > count2) { + LedTOGGLE; + //Control_Cycle(); + //HW_Security(); + //Write_Actuator(); + count = 0; + } + if (count == 1023) { count = 0; } } else { DISABLE_MOTOR; - 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 + 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 //LedTOGGLE; } } @@ -39,21 +46,22 @@ 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 - } + // 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); + }*/ - //------EEPROM initial values------------- - Restore_EepromVAR(); - // get value of Motor ID - //rs485_address = eepromVAR[ID]; + //------EEPROM initial values------------- + /*Restore_EepromVAR(); + //blinkLedN(5); + // get value of Motor ID + rs485_address = eepromVAR[ID];*/ // configure AVR chip i/o Config_Hardware(); @@ -64,20 +72,8 @@ 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);