diff --git a/servo_firmware/ax12/Makefile b/servo_firmware/ax12/Makefile index 2d1411a4af7a15310e7fb0c0484a1ebdae5f4bad..8bdf19a7477384333d9c2fd04ea10ab21736832d 100755 --- a/servo_firmware/ax12/Makefile +++ b/servo_firmware/ax12/Makefile @@ -1,4 +1,4 @@ -PROJECT=ax12_fw +PROJECT=rx28_fw ######################################################## # afegir tots els fitxers que s'han de compilar aquà ######################################################## @@ -30,7 +30,7 @@ $(PROJECT).elf: $(OBJS) $(CC) -c $(CFLAGS) -I$(INCLUDE_DIR) -o $@ $< download: $(MAIN_OUT_HEX) - ../../bin/fw_downloader -d /dev/ttyUSB0 -f $(PROJECT).hex -s ax12 + ../../bin/fw_downloader -d /dev/ttyUSB0 -f $(PROJECT).hex -s rx28 clean: rm $(PROJECT).* diff --git a/servo_firmware/ax12/include/CFG_HW_Dynamixel.h b/servo_firmware/ax12/include/CFG_HW_Dynamixel.h index 1c7d6b1c6ea8ba81f528f03d5e4ac548d3b2f82f..cee708ddc68e992f8e02c5deb912d4140ef682f5 100755 --- a/servo_firmware/ax12/include/CFG_HW_Dynamixel.h +++ b/servo_firmware/ax12/include/CFG_HW_Dynamixel.h @@ -1,18 +1,26 @@ #ifndef _CFG_HW_DYNAMIXEL_H #define _CFG_HW_DYNAMIXEL_H + +#define LED_PIN PD2 +#define ENABLE_MOTOR_PIN PD7 +#define CW_PWM_MOTOR_PIN PB1 +#define CCW_PWM_MOTOR_PIN PB2 + // this is for debuging tonggle LED i times // void blinkLedN(uint8_t i); -// this is for debuging => tonggle LED -//void LedTONGGLE(void) ; -// this is for debuging => off LED -//void LedOFF(void) ; -// this is for debugging => on LED -//void LedON(void) ; - -#define LedOFF(x) PORTD |= (1<<x) // off the LED -#define LedON(x) PORTD &= ~(1<<x) // ON the LED -#define LedTOGGLE(x) PORTD ^= _BV(x) // Toggle PD2 led + + +#define LedOFF PORTD |= (1<<LED_PIN) // off LED +#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 ENABLE_MOTOR PORTD &= ~(1<<ENABLE_MOTOR_PIN) // enable motor +#define DISABLE_MOTOR PORTD |= (1<<ENABLE_MOTOR_PIN) // disable motor + void Config_Hardware(void); diff --git a/servo_firmware/ax12/src/CFG_HW_Dynamixel.c b/servo_firmware/ax12/src/CFG_HW_Dynamixel.c index 1d203ef868de2132c4bdbac81172ba1aeccfbf7a..5eda0e922896e78cf5e8314c9f90ed036a45466b 100755 --- a/servo_firmware/ax12/src/CFG_HW_Dynamixel.c +++ b/servo_firmware/ax12/src/CFG_HW_Dynamixel.c @@ -8,13 +8,13 @@ { uint8_t i; - LedOFF(PD2); // PD2 led led OFF + LedOFF; // led OFF _delay_ms(500); for (i=1; i<=(2*j); i++) { - LedTOGGLE(PD2); // tonggle PD2 led + LedTOGGLE; // tonggle led _delay_ms(200); } - LedOFF(PD2); // PD2 led led OFF + LedOFF; // led OFF _delay_ms(500); }*/ @@ -22,12 +22,14 @@ void Config_Hardware(void) { - DDRD |= (1 << PD2); // Set LED as output - LedON(PD2); // ON the LED - DDRB |= (1 << PB1); // Set CW as output - OCR1A = 0x0000; //PB1 => set PWM for X% duty cycle @ 10bit - DDRB |= (1 << PB2); // Set CCW as output - OCR1B = 0x0000; //PB2 => set PWM for Y% duty cycle @ 10bit + 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 diff --git a/servo_firmware/ax12/src/TXRX_Dynamixel.c b/servo_firmware/ax12/src/TXRX_Dynamixel.c index 8b061f9ee22707bc202a79b268e2d490dc25ddc6..56a5a58591633cedf1b6df51d79a614dcbd98158 100755 --- a/servo_firmware/ax12/src/TXRX_Dynamixel.c +++ b/servo_firmware/ax12/src/TXRX_Dynamixel.c @@ -63,8 +63,8 @@ void init_RS485(void) { // configure the IO ports // DDRD - Port D Data Direction Register - DDRD=DDRD|0xC2;// RX_en, TX_en, TX are outputs - DDRD=DDRD&0xFE;// RX is an input - LED OFF + DDRD=DDRD|0xC2;// RX_en, TX_en, TX are outputs 0xC2=11000010 + DDRD=DDRD&0xFE;// RX is an input 0xFE=11111110 // configure the ports to receive data SET_RS485_RXD; diff --git a/servo_firmware/ax12/src/main.c b/servo_firmware/ax12/src/main.c index af990fb2f217841e8eaf4ff315433deeff576e73..51fc747666db09c662bf93ada42fff2a4b75c0e1 100755 --- a/servo_firmware/ax12/src/main.c +++ b/servo_firmware/ax12/src/main.c @@ -14,7 +14,7 @@ //*************CTRL = INTERRUPT FUNCTION ********************************* uint16_t count = 0; uint16_t count2 = 0; -ISR( TIMER0_OVF_vect) { +/*ISR( TIMER0_OVF_vect) { int16_t TorqueEnable; sei(); @@ -38,47 +38,47 @@ ISR( TIMER0_OVF_vect) { 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; // 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, + /*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 }; + 84, 85, 86 };*/ // ini eeprom if first time after run reflash system - if (eeprom_read_byte((uint8_t*) Gate_Restore_Eeprom) != 0xCC) { + /*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(); + /*Restore_EepromVAR(); //blinkLedN(5); // get value of Motor ID - rs485_address = eepromVAR[ID]; + rs485_address = eepromVAR[ID];*/ // configure AVR chip i/o Config_Hardware(); - // asign actual position to goal position and asume zero motor turns + /*// asign actual position to goal position and asume zero motor turns Ini_Position(); // initialize the RS485 interface - init_RS485(); + init_RS485();*/ // end of inicialization - LedOFF(PD2); + LedOFF; while (1) { - //LedOFF(PD2); - status = RxRS485Packet(&id, &instruction, &length, data); + //LedOFF; + /*status = RxRS485Packet(&id, &instruction, &length, data); if (status == CHECK_ERROR) { TxRS485Packet(rs485_address, CHECKSUM_ERROR, 0, NULL); } else if (status == CORRECT) { - //LedON(PD2); + //LedON; if (id == rs485_address) { switch (instruction) { //*********************************************************** @@ -118,7 +118,7 @@ int16_t main(void) { break; //................................................ case 2: // case WORD - //LedTOGGLE(PD2); + //LedTOGGLE; TxRx_Read_word_Dynamixel_RAM(data[0], answer); break; //................................................ default: @@ -217,9 +217,9 @@ int16_t main(void) { switch (data[0]) { // aditional instructions for particular registers in ram case LED: // case RAM and BYTE if (Read_byte_Dynamixel_RAM(LED) == 1) { - LedON(PD2); + LedON; } else { - LedOFF(PD2); + LedOFF; } break; //................................................ default: @@ -251,7 +251,7 @@ int16_t main(void) { //*************************************************** } } - } + }*/ } }