Camille Muller
/
PFE_MOTORISATION
PFE_MULLER_CAMILLE
main.cpp
- Committer:
- mullercamille
- Date:
- 2016-02-12
- Revision:
- 0:5fb36f7e2cfb
File content as of revision 0:5fb36f7e2cfb:
#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()); } }