rx28 firmware ctrl cycle work - multiple turns - PD controler - no space for PID

servo_firmware/rx28/src/main.c

 #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);