rx28 firmware interrupt ctrl cycle and read potenciometer work

servo_firmware/rx28/Makefile

@@ -3,8 +3,8 @@ 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/CTRL_Dynamixel.c src/MEM_Dynamixel.c src/CFG_HW_Dynamixel.c
-SOURCES=src/main.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/

servo_firmware/rx28/src/main.c

 #include <avr/io.h>
 #include <util/delay.h>
-//#include <avr/interrupt.h>
+#include <avr/interrupt.h>
 //#include <avr/eeprom.h>
 #include "CFG_HW_Dynamixel.h"
 //#include "TXRX_Dynamixel.h"
-//#include "CTRL_Dynamixel.h"
-//#include "MEM_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;
 
- 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
- }
- }*/
+ISR( TIMER0_OVF_vect) {
+	int16_t TorqueEnable;
+
+	sei();// enable all interrupts
+	//cli(); // disable all interrupts just to make sure
+	TorqueEnable = Read_byte_Dynamixel_RAM(Torque_Enable);
+	if (TorqueEnable == 1) {
+	//if (0) {
+		//ENABLE_MOTOR;
+		DISABLE_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) {
+			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
+		//LedTOGGLE;
+	}
+}
 
 //****************    M A I N  *********************************
 int16_t main(void) {
-	/*unsigned char data[128], id, length, instruction, answer[2], status,
-			en_vector, i, j;*/
-	uint16_t ii, jj;
-	//double pwm;
+	 unsigned char data[128], id, length, instruction, answer[2], status,
+			en_vector, i, j;
 	// 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,
@@ -59,31 +63,16 @@ int16_t main(void) {
 	 // get value of Motor ID
 	 rs485_address = eepromVAR[ID];*/
 
-	// configure AVR chip i/o
+	 	 // configure AVR chip i/o
 	Config_Hardware();
-	/*// asign actual position to goal position and asume zero motor turns
-	 Ini_Position();
-
-	 // initialize the RS485 interface
-	 init_RS485();*/
+		// asign actual position to goal position and asume zero motor turns
+	Ini_Position();
+		// initialize the RS485 interface
+	 /*init_RS485();*/
 
 	// end of inicialization
 	LedOFF;
-	ENABLE_MOTOR;
-	//DISABLE_MOTOR;
-	ii = 900;
-	jj = 0;
-	OCR1A = 0;
 	while (1) {
-		//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) {