diff --git a/servo_firmware/ax12/src/motor_control.c b/servo_firmware/ax12/src/motor_control.c
index d2b9a00a504c47650187ff3a74b99895b8d96273..0c164f286b658a4229dd4e2fe58cc7e47f41e2c2 100644
--- a/servo_firmware/ax12/src/motor_control.c
+++ b/servo_firmware/ax12/src/motor_control.c
@@ -65,7 +65,6 @@ void motor_control_loop(void)
{
// update control loop
// get all necessary variables
- gpio_led_on();
current_position=ram_data[Present_Position_L]+(ram_data[Present_Position_H]<<8);
current_velocity = current_position - previous_position;
ram_data[Present_Speed_L]=current_velocity&0xFF;
@@ -102,6 +101,7 @@ void motor_control_loop(void)
/* generate the intermediate target point */
if(count==CONTROL_COUNT && seek_delta!=seek_position && seek_velocity>0) // Tick is our time constant
{
+ gpio_led_on();
if(start_position<seek_position)
{
seek_delta_shift+=seek_velocity;
@@ -116,6 +116,7 @@ void motor_control_loop(void)
if(seek_delta<=seek_position)
seek_delta=seek_position;
}
+ gpio_led_off();
}
/* compute the error */
p_component = seek_delta - current_position;
@@ -164,6 +165,5 @@ void motor_control_loop(void)
count=0;
else
count++;
- gpio_led_off();
}
}
diff --git a/src/fw_downloader/fw_downloader.cpp b/src/fw_downloader/fw_downloader.cpp
index a89ef02db34fd11fe61b1793d4c39662dee75b77..f41efc83e46febd1b57c24bac8db40c97e385400 100755
--- a/src/fw_downloader/fw_downloader.cpp
+++ b/src/fw_downloader/fw_downloader.cpp
@@ -11,7 +11,8 @@
const unsigned char bootloader_ping='#';
const unsigned char bootloader_ack[]="\n";
-const unsigned char bootloader_load_hex[]="l 0x08003000\n";
+//const unsigned char bootloader_load_hex[]="l 0x08003000\n";
+const unsigned char bootloader_load_hex[]="l";
const unsigned char bootloader_load_motion[]="l 0x08060000\n";
const unsigned char bootloader_go[]="\ngo\n";
@@ -157,60 +158,63 @@ int main(int argc,char *argv[])
fclose(motion_fd);
}
printf("Press DARwIn-OP's Reset button to start...\n");
- while(!bootloader_connected)
+ if(servo_model!="cm9")
{
- try{
- event_server->wait_all(events,20);
- num_data=serial_port.get_num_data();
- serial_port.read((unsigned char *)bootloader_data,num_data);
- bootloader_data[num_data]='\0';
- printf("%s",bootloader_data);
- fflush(stdout);
- for(i=0;i<num_data;i++)
- {
- if(servo_model=="mx28")
+ while(!bootloader_connected)
+ {
+ try{
+ event_server->wait_all(events,20);
+ num_data=serial_port.get_num_data();
+ serial_port.read((unsigned char *)bootloader_data,num_data);
+ bootloader_data[num_data]='\0';
+ printf("%s",bootloader_data);
+ fflush(stdout);
+ for(i=0;i<num_data;i++)
{
- if(bootloader_data[i]=='#')
+ if(servo_model=="mx28")
{
- count++;
- if(count==5)
- bootloader_connected=true;
+ if(bootloader_data[i]=='#')
+ {
+ count++;
+ if(count==5)
+ bootloader_connected=true;
+ }
}
- }
- else if(servo_model=="ax12" || servo_model=="rx28")
- {
- if(bootloader_data[i]=='*')
+ else if(servo_model=="ax12" || servo_model=="rx28")
{
- count++;
- if(count==5)
- bootloader_connected=true;
+ if(bootloader_data[i]=='*')
+ {
+ count++;
+ if(count==5)
+ bootloader_connected=true;
+ }
}
}
+ }catch(CEventTimeoutException &e){
+ // do nothing and retry
+ serial_port.write((unsigned char *)&bootloader_ping, 1);
}
- }catch(CEventTimeoutException &e){
- // do nothing and retry
- serial_port.write((unsigned char *)&bootloader_ping, 1);
}
- }
- serial_port.write((unsigned char *)bootloader_ack, 1);
- while(!end)
- {
- try{
- event_server->wait_all(events,3000);
- num_data=serial_port.get_num_data();
- serial_port.read(bootloader_data,num_data);
- bootloader_data[num_data]='\0';
- printf("%s",bootloader_data);
- fflush(stdout);
- }catch(CEventTimeoutException &e){
- // the flash has been properly erased
- end=true;
+ serial_port.write((unsigned char *)bootloader_ack, 1);
+ while(!end)
+ {
+ try{
+ event_server->wait_all(events,3000);
+ num_data=serial_port.get_num_data();
+ serial_port.read(bootloader_data,num_data);
+ bootloader_data[num_data]='\0';
+ printf("%s",bootloader_data);
+ fflush(stdout);
+ }catch(CEventTimeoutException &e){
+ // the flash has been properly erased
+ end=true;
+ }
}
}
sleep(1);
end=false;
// start the download
- serial_port.write((unsigned char *)bootloader_load_hex,13);
+ serial_port.write((unsigned char *)bootloader_load_hex,1);
while(!end)
{
try{
@@ -352,19 +356,28 @@ int main(int argc,char *argv[])
while(downloaded_size < binary_size)
{
usleep(1000);
+ if((num_data=serial_port.get_num_data())!=0)
+ {
+ serial_port.read(bootloader_data,num_data);
+ bootloader_data[num_data]='\0';
+ printf("%s",bootloader_data);
+ return 0;
+ }
block_size=binary_size-downloaded_size;
- if(block_size>64)
- block_size=64;
+// if(block_size>64)
+// block_size=64;
+ block_size=binary_size;
serial_port.write(&binary_file[start_address+downloaded_size],block_size);
downloaded_size+=block_size;
printf("\rDownloading Firmware (%ld/%ld byte)", downloaded_size, binary_size);
+ sync();
}
- printf("\nDownload complete\n");
// send the checksum
serial_port.write(&checksum,1);
sleep(1);
+ printf("\nDownload complete\n");
end=false;
- while(!end)
+/* while(!end)
{
try{
event_server->wait_all(events,3000);
@@ -377,7 +390,8 @@ int main(int argc,char *argv[])
// no error to report
end=true;
}
- }
+ }*/
+ return 0;
if(opt_fw_file.size()!=0)
{
// start the loaded program
diff --git a/src/robotis_mtn_file/CMakeLists.txt b/src/robotis_mtn_file/CMakeLists.txt
index 5e9ceb436fd1bc76dfe5134e1ef005350fc8033e..dec91906422c8dc8366b5922b6e6c40e8c625e4d 100644
--- a/src/robotis_mtn_file/CMakeLists.txt
+++ b/src/robotis_mtn_file/CMakeLists.txt
@@ -42,8 +42,6 @@ INSTALL(TARGETS robotis_mtn_parser
ARCHIVE DESTINATION lib)
INSTALL(FILES ${headers} DESTINATION include)
INSTALL(FILES ${CMAKE_CURRENT_BINARY_DIR}/mtn_parser.hpp DESTINATION include)
-INSTALL(FILES ${CMAKE_CURRENT_BINARY_DIR}/location.hh DESTINATION include)
-INSTALL(FILES ${CMAKE_CURRENT_BINARY_DIR}/position.hh DESTINATION include)
INSTALL(FILES ${CMAKE_CURRENT_BINARY_DIR}/stack.hh DESTINATION include)
INSTALL(FILES ../Findrobotis_mtn_parser.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
ADD_SUBDIRECTORY(examples)
diff --git a/src/robotis_mtn_file/mtn_file.y b/src/robotis_mtn_file/mtn_file.y
index b94beb8843730da723c33050c0bdcd782a596f6c..c8dca07fc754a5ec8f107af9b3a8b494850a3f3f 100644
--- a/src/robotis_mtn_file/mtn_file.y
+++ b/src/robotis_mtn_file/mtn_file.y
@@ -1,8 +1,8 @@
%skeleton "lalr1.cc"
%require "2.5"
%defines
-%define namespace "MTN"
-%define parser_class_name "MTN_Parser"
+%define api.namespace {MTN}
+%define parser_class_name {MTN_Parser}
%name-prefix "mtn_"
%code requires
@@ -149,7 +149,7 @@ angles: angles INT { angles.push_back($2); }
;
%%
void
-MTN::MTN_Parser::error(const MTN::MTN_Parser::location_type &l,const std::string &err_message )
+MTN::MTN_Parser::error(const std::string& err_message)
{
std::cerr << "Error: " << err_message << "\n";
}
diff --git a/src/robotis_mtn_file/mtn_file_parser.cpp b/src/robotis_mtn_file/mtn_file_parser.cpp
index 1dd6a80fa0f5b6efb7e20412be8f20ac2efb4ce9..2d01bf0f12f2c72e05eba4e3a06a2e09cdc52ed8 100644
--- a/src/robotis_mtn_file/mtn_file_parser.cpp
+++ b/src/robotis_mtn_file/mtn_file_parser.cpp
@@ -1,6 +1,7 @@
#include <cctype>
#include <fstream>
#include <cassert>
+#include <unistd.h>
#include "mtn_file_parser.hpp"
#include "mtn_parser.hpp"
@@ -29,15 +30,15 @@ void MTN::CMtnFileParser::parse(const char *filename)
{
std::ifstream in_file(filename);
if(!in_file.good())
- exit( EXIT_FAILURE );
+ _exit( -1 );
if(this->scanner!=NULL)
delete this->scanner;
try
{
scanner=new MTN::CMtnFileScanner(&in_file);
}catch(std::bad_alloc &ba){
- std::cerr << "Failed to allocate scanner: (" << ba.what() << "), exiting!!\n";
- exit( EXIT_FAILURE );
+ std::cerr << "Failed to allocate scanner: (" << ba.what() << "), _exiting!!\n";
+ _exit( -1 );
}
if(this->parser!=NULL)
delete this->parser;
@@ -45,8 +46,8 @@ void MTN::CMtnFileParser::parse(const char *filename)
{
parser = new MTN::MTN_Parser((*scanner),(*this));
}catch(std::bad_alloc &ba){
- std::cerr << "Failed to allocate parser: (" << ba.what() << "), exiting!!\n";
- exit( EXIT_FAILURE );
+ std::cerr << "Failed to allocate parser: (" << ba.what() << "), _exiting!!\n";
+ _exit( -1 );
}
const int accept(0);