main.cpp

00001 #include "mbed.h"
00002 
00003 
00004 #define ADR_MOTEUR_NUMBER 0
00005 #define ADR_MODULE 1
00006 
00007 
00008 
00009 Serial pc(USBTX, USBRX); // tx, rx
00010 Serial serial_(PA_9,PA_10);
00011 
00012 
00013 struct s_val
00014 {
00015     char v1;
00016     char v2;
00017     char v3;
00018     char v4;
00019 }typedef val;
00020 
00021 enum reply_code
00022 {
00023 success =100, //Successfully executed, no error
00024 loaded_into_TMCL= 101, // Command loaded into TMCL™program EEPROM
00025 wrong_checksum = 1, //Wrong checksum
00026 invalid_cmd = 2, //Invalid command
00027 wrong_type =3, // Wrong type
00028 invalid_value = 4, // Invalid value
00029 eeprom_locked = 5,  //Configuration EEPROM locked
00030 cmd_non_available = 6 // Command not available
00031 };
00032 
00033 
00034 
00035 enum type_commande{
00036   
00037 // Motion command
00038         ROL = 2, // Rotate left
00039         ROR = 1, // Rotate right
00040         MVP = 4, // Move to position
00041         MST = 3, // Motor stop
00042         RFS = 13, // Reference search
00043         SCO = 30, // Store coordinate
00044         CCO = 32, // Capture coordinate
00045         GCO = 31, // Get coordinate
00046 // Parameter command 
00047         SAP = 5, // Set axis parameter
00048         GAP = 6, // Get axis parameter
00049         STAP = 7, // Store axis parameter into EEPROM
00050         RSAP = 8, // Restore axis parameter from EEPROM
00051         SGP =  9, //Set global parameter
00052         GGP = 10, // Get global parameter
00053         STGP = 11, // Store global parameter into EEPROM
00054         RSGP = 12, // Restore global parameter from EEPROM  
00055 // IO command 
00056         SIO = 14, // Set output
00057         GIO = 15, //Get input
00058     };
00059 
00060 struct s_cmd
00061 {
00062 unsigned char module_adr;
00063 enum type_commande command_number;
00064 unsigned char type_number;
00065 unsigned char motor_or_bank_number;
00066 val val;
00067 unsigned char checksum;
00068 }typedef cmd;
00069 
00070 struct s_reply
00071 {
00072 unsigned char reply_adr;
00073 unsigned char motor_or_bank_number;
00074 unsigned char status;
00075 enum type_commande command_number;
00076 int val;
00077 unsigned char checksum;
00078 }typedef reply;
00079 
00080 
00081     
00082 
00083 
00084 // Add checksum to command
00085 cmd* add_cs(cmd* Command)
00086 { 
00087 unsigned char Checksum;
00088 //Set the “Command” array to the desired command
00089 Checksum = Command->module_adr;
00090 Checksum+=Command->command_number;
00091 Checksum+=Command->type_number;
00092 Checksum+=Command->motor_or_bank_number;
00093 Checksum+=Command->val.v1;
00094 Checksum+=Command->val.v2;
00095 Checksum+=Command->val.v3;
00096 Checksum+=Command->val.v4;
00097 
00098 Command->checksum =Checksum; //insert checksum as last byte of the command
00099 //Now, send it to the module
00100 return Command;
00101 }
00102 
00103 void reverse_val(int speed,val *valeur)
00104 {
00105     char t1,t2,t3,t4;
00106  ((int*)valeur)[0] = speed;
00107     
00108 t1 = valeur->v1; 
00109 t2 = valeur->v2;
00110 t3 = valeur->v3;
00111 t4 = valeur->v4;
00112 valeur->v1 = t4; 
00113 valeur->v2 = t3;
00114 valeur->v3 = t2;
00115 valeur->v4 = t1;
00116     
00117 }
00118 
00119 
00120 cmd* cmd_rotate_left(int speed)
00121 {
00122     cmd* tab = (cmd *) malloc(sizeof(s_cmd));;
00123     tab->motor_or_bank_number = ADR_MOTEUR_NUMBER;
00124     tab->module_adr = ADR_MODULE;
00125     tab->command_number = ROL; // -> LEFT
00126     reverse_val(speed,&(tab->val));
00127     tab->type_number = 0; // don't care
00128     
00129     return add_cs(tab);
00130 }
00131 
00132 cmd* cmd_rotate_right(int speed)
00133 {
00134     cmd* tab = (cmd *) malloc(sizeof(s_cmd));;
00135     tab->motor_or_bank_number = ADR_MOTEUR_NUMBER;
00136     tab->module_adr = ADR_MODULE;
00137     tab->command_number = ROR;
00138     reverse_val(speed,&(tab->val));
00139     tab->type_number = 0; // don't care
00140 
00141     return add_cs(tab);
00142 }
00143 
00144 cmd* cmd_stop()
00145 {   
00146     cmd* tab = (cmd *) malloc(sizeof(s_cmd));;
00147     tab->motor_or_bank_number = ADR_MOTEUR_NUMBER;
00148     tab->module_adr = ADR_MODULE;
00149     tab->command_number = MST;
00150     //tab->val = (val) 0; // Don't care
00151     tab->type_number = 0; // don't care
00152     return add_cs(tab);
00153 }
00154 
00155 
00156 void send_data(Serial* unport,cmd * cmd)
00157 {
00158 int itr =0;
00159 for( ; itr < 9; itr++)
00160 {
00161 char val= ((char) ((const char*)cmd)[itr]);
00162 unport->putc(val);
00163 }
00164 }
00165 
00166 int main() {
00167     
00168   wait(0.5);
00169   pc.baud(9600);
00170     serial_.baud(9600);
00171     
00172     
00173     cmd * cmd = cmd_rotate_right(350);
00174     send_data(&serial_,cmd);
00175    while(1)
00176      {
00177          if(serial_.readable())
00178          pc.putc(serial_.getc());
00179      }
00180 
00181 }
00182 
00183