Diff: main.cpp

Revision:

0:5fb36f7e2cfb

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Feb 12 17:45:09 2016 +0000
@@ -0,0 +1,183 @@
+#include "mbed.h"
+
+
+#define ADR_MOTEUR_NUMBER 0
+#define ADR_MODULE 1
+
+
+
+Serial pc(USBTX, USBRX); // tx, rx
+Serial serial_(PA_9,PA_10);
+
+
+struct s_val
+{
+    char v1;
+    char v2;
+    char v3;
+    char v4;
+}typedef val;
+
+enum reply_code
+{
+success =100, //Successfully executed, no error
+loaded_into_TMCL= 101, // Command loaded into TMCL™program EEPROM
+wrong_checksum = 1, //Wrong checksum
+invalid_cmd = 2, //Invalid command
+wrong_type =3, // Wrong type
+invalid_value = 4, // Invalid value
+eeprom_locked = 5,  //Configuration EEPROM locked
+cmd_non_available = 6 // Command not available
+};
+
+
+
+enum type_commande{
+  
+// Motion command
+        ROL = 2, // Rotate left
+        ROR = 1, // Rotate right
+        MVP = 4, // Move to position
+        MST = 3, // Motor stop
+        RFS = 13, // Reference search
+        SCO = 30, // Store coordinate
+        CCO = 32, // Capture coordinate
+        GCO = 31, // Get coordinate
+// Parameter command 
+        SAP = 5, // Set axis parameter
+        GAP = 6, // Get axis parameter
+        STAP = 7, // Store axis parameter into EEPROM
+        RSAP = 8, // Restore axis parameter from EEPROM
+        SGP =  9, //Set global parameter
+        GGP = 10, // Get global parameter
+        STGP = 11, // Store global parameter into EEPROM
+        RSGP = 12, // Restore global parameter from EEPROM  
+// IO command 
+        SIO = 14, // Set output
+        GIO = 15, //Get input
+    };
+
+struct s_cmd
+{
+unsigned char module_adr;
+enum type_commande command_number;
+unsigned char type_number;
+unsigned char motor_or_bank_number;
+val val;
+unsigned char checksum;
+}typedef cmd;
+
+struct s_reply
+{
+unsigned char reply_adr;
+unsigned char motor_or_bank_number;
+unsigned char status;
+enum type_commande command_number;
+int val;
+unsigned char checksum;
+}typedef reply;
+
+
+    
+
+
+// Add checksum to command
+cmd* add_cs(cmd* Command)
+{ 
+unsigned char Checksum;
+//Set the “Command” array to the desired command
+Checksum = Command->module_adr;
+Checksum+=Command->command_number;
+Checksum+=Command->type_number;
+Checksum+=Command->motor_or_bank_number;
+Checksum+=Command->val.v1;
+Checksum+=Command->val.v2;
+Checksum+=Command->val.v3;
+Checksum+=Command->val.v4;
+
+Command->checksum =Checksum; //insert checksum as last byte of the command
+//Now, send it to the module
+return Command;
+}
+
+void reverse_val(int speed,val *valeur)
+{
+    char t1,t2,t3,t4;
+ ((int*)valeur)[0] = speed;
+    
+t1 = valeur->v1; 
+t2 = valeur->v2;
+t3 = valeur->v3;
+t4 = valeur->v4;
+valeur->v1 = t4; 
+valeur->v2 = t3;
+valeur->v3 = t2;
+valeur->v4 = t1;
+    
+}
+
+
+cmd* cmd_rotate_left(int speed)
+{
+    cmd* tab = (cmd *) malloc(sizeof(s_cmd));;
+    tab->motor_or_bank_number = ADR_MOTEUR_NUMBER;
+    tab->module_adr = ADR_MODULE;
+    tab->command_number = ROL; // -> LEFT
+    reverse_val(speed,&(tab->val));
+    tab->type_number = 0; // don't care
+    
+    return add_cs(tab);
+}
+
+cmd* cmd_rotate_right(int speed)
+{
+    cmd* tab = (cmd *) malloc(sizeof(s_cmd));;
+    tab->motor_or_bank_number = ADR_MOTEUR_NUMBER;
+    tab->module_adr = ADR_MODULE;
+    tab->command_number = ROR;
+    reverse_val(speed,&(tab->val));
+    tab->type_number = 0; // don't care
+
+    return add_cs(tab);
+}
+
+cmd* cmd_stop()
+{   
+    cmd* tab = (cmd *) malloc(sizeof(s_cmd));;
+    tab->motor_or_bank_number = ADR_MOTEUR_NUMBER;
+    tab->module_adr = ADR_MODULE;
+    tab->command_number = MST;
+    //tab->val = (val) 0; // Don't care
+    tab->type_number = 0; // don't care
+    return add_cs(tab);
+}
+
+
+void send_data(Serial* unport,cmd * cmd)
+{
+int itr =0;
+for( ; itr < 9; itr++)
+{
+char val= ((char) ((const char*)cmd)[itr]);
+unport->putc(val);
+}
+}
+
+int main() {
+    
+  wait(0.5);
+  pc.baud(9600);
+    serial_.baud(9600);
+    
+    
+    cmd * cmd = cmd_rotate_right(350);
+    send_data(&serial_,cmd);
+   while(1)
+     {
+         if(serial_.readable())
+         pc.putc(serial_.getc());
+     }
+
+}
+
+