Camille Muller
/
PFE_MOTORISATION
PFE_MULLER_CAMILLE
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()); + } + +} + +