Chris LU
/
Self_Riding_Bicycle
2018/06/08
Revision 0:bf9bf4b7625f, committed 2018-06-08
- Comitter:
- cpul5338
- Date:
- Fri Jun 08 14:11:49 2018 +0000
- Commit message:
- 2018/06/08
Changed in this revision
diff -r 000000000000 -r bf9bf4b7625f Controller.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Controller.cpp Fri Jun 08 14:11:49 2018 +0000 @@ -0,0 +1,72 @@ +#include "Controller.h" +#include "SystemConstant.h" +#include "SensorFusion.h" + +bool test1 = 0; +float sigma = 0.0; +float alpha_1 = 0.0; +float alpha_2 = 0.0; +float K_1[3] = {1175.5, 302.2, -27.7}; +float K_2[3] = {193.573, 50.0229, -2.8192}; +float K_LQR01[3] = {39.9682, 9.2497, -0.0712}; // R = 1 +float K_LQR11[3] = {107.3029, 26.5432, -2.1435}; // R = 0.01 +float K_LQR21[3] = {285.88, 72.6362, -7.5105}; // R = 0.001 +float K_LQR31[3] = {444.0651, 106.7309, -12.7130}; // R = 0.0001 +float K_LQR75[3] = {789.0790, 190.093 -23.4674}; // R = 0.0003 +float K_LQR755[3]= {981.915, 252.3768, -28.3543}; // R = 0.000075 +float K_LQR655[3]= {1053.0382, 270.7444, -30.4835}; // R = 0.00065 +float K_LQR55[3] = {1197.4, 308.0321, -34.8059}; // R = 0.00005 +float K_LQR65[3] = {1095.0974, 281.6061, -31.7426}; // R = 0.00006 +float K_LQR35[3] = {1539.18735, 396.2928, -45.0369}; // R = 0.00006 +float K_LQR15[3] = {2649.2, 682.9531, -78.2644}; // R = 0.00001 +float K_LQR85[3] = {951.4667, 244.5136, -27.4427}; // R = 0.00008 +float K_LQR95[3] = {898.3697, 230.8014, -25.8531}; // R = 0.00009 +float u_1 = 0.0; +float u_2 = 0.0; +float u_3 = 0.0; +float u_d = 0.0; +float u = 0.0; +float roll_ref = 0.0; +float roll_err = 0.0; +float steer_ref = 0.0; +float steer_ref_old = 0.0; +float steer_rad = 0.0; +float steer_rad_old = 0.0; +float steer_degree = 0.0; +float steering_angle = 0.0f; + + +//************************************************************************************************************************************************************************************************************************* +// Controller +void controller(float velocity) +{ + roll_err = roll_angle - roll_ref; + + // Controller from Linear Quadratic Regulator +// u_1 = - K_LQR75[0]*roll_err; +// u_2 = - K_LQR655[1]*droll_angle; +// u_3 = - K_LQR75[2]*(steer_rad-steer_ref); +// u = u_1 + u_2 + u_3 - 18.8803f*roll_ref; + + + // Controller from Linear Matrice Inequality + sigma = (velocity - Vmin) / (Vmax - Vmin); + alpha_1 = (1 - sigma); + alpha_2 = sigma; + u_1 = -K_1[0]*roll_err*alpha_1 - K_2[0]*roll_err*alpha_2; + u_2 = -K_1[1]*droll_angle*alpha_1 - K_2[1]*droll_angle*alpha_2; + u_3 = -K_1[2]*(steer_rad-steer_ref)*alpha_1 - K_2[2]*(steer_rad-steer_ref)*alpha_2; + u = u_1 + u_2 + u_3 - 18.8803f*roll_ref; + +}// controller + +//************************************************************************************************************************************************************************************************************************* +// steer_angle +void steer_angle(float u_in, float velocity) +{ + steer_rad = lpf(u_in, steer_rad_old, u2steer_lpf_freq * velocity) * u2steer_gain / velocity; + steer_rad_old = steer_rad; + steering_angle = steer_rad/3.14f*180.0f; + if(steering_angle > 60) steering_angle = 60; + if(steering_angle <-60) steering_angle = -60; +}// steer_angle \ No newline at end of file
diff -r 000000000000 -r bf9bf4b7625f Controller.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Controller.h Fri Jun 08 14:11:49 2018 +0000 @@ -0,0 +1,43 @@ +#include "mbed.h" +#include "SystemConstant.h" +#include "SensorFusion.h" + +#ifndef CONTROLLER_H_INCLUDE +#define CONTROLLER_H_INCLUDE +extern bool test1; +extern float sigma; +extern float alpha_1; +extern float alpha_2; +extern float roll_err; +extern float K_1[3]; +extern float K_2[3]; +extern float K_LQR01[3]; +extern float K_LQR11[3]; +extern float K_LQR21[3]; +extern float K_LQR31[3]; +extern float K_LQR75[3]; +extern float K_LQR55[3]; +extern float K_LQR65[3]; +extern float K_LQR35[3]; +extern float K_LQR15[3]; +extern float K_LQR85[3]; +extern float K_LQR95[3]; +extern float K_LQR655[3]; +extern float K_LQR755[3]; +extern float u_1; +extern float u_2; +extern float u_3; +extern float u_d; +extern float u; +extern float roll_ref; +extern float steer_ref; +extern float steer_ref_old; +extern float steer_rad; +extern float steering_angle; +extern float steer_rad_old; +extern float steer_degree; + +extern void controller(float velocity); +extern void steer_angle(float u_in, float velocity); +extern void anti_widup(void); +#endif// CONTROLLER_H_INCLUDE
diff -r 000000000000 -r bf9bf4b7625f LSM9DS0.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LSM9DS0.lib Fri Jun 08 14:11:49 2018 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/users/cpul5338/code/LSM9DS0/#b9e5363b8c38
diff -r 000000000000 -r bf9bf4b7625f Mx28.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Mx28.cpp Fri Jun 08 14:11:49 2018 +0000 @@ -0,0 +1,1024 @@ +#include <stdlib.h> +#include <stdio.h> +#include "mbed.h" +#include "Mx28.h" + + + unsigned char Instruction_Packet_Array[15]; // Array to hold instruction packet data + unsigned char Status_Packet_Array[8]; // Array to hold returned status packet data + unsigned int Time_Counter; // Timer for time out watchers + unsigned char Direction_Pin; // Pin to control TX/RX buffer chip + unsigned char Status_Return_Value = READ; // Status packet return states ( NON , READ , ALL ) + +//------------------------------------------------------------------------------------------------------------------------------- +// Private Methods +//------------------------------------------------------------------------------------------------------------------------------- +void DynamixelClass::debugframe(void) { + for (int i=0; i<10; i++) + printf("%x,",Instruction_Packet_Array[i]); + printf("\r\n"); +} + +void DynamixelClass::debugStatusframe(void) { + for (int i=0; i<10; i++) + printf("%x,",Status_Packet_Array[i]); + printf("\r\n"); +} + + +void DynamixelClass::transmitInstructionPacket(void){ // Transmit instruction packet to Dynamixel + + unsigned char Counter; + Counter = 0; + servoSerialDir->write(1); // Set TX Buffer pin to HIGH + + servoSerial->putc(HEADER); // Write Header (0xFF) data 1 to serial + servoSerial->putc(HEADER); // Write Header (0xFF) data 2 to serial + servoSerial->putc(Instruction_Packet_Array[0]); // Write Dynamixal ID to serial + servoSerial->putc(Instruction_Packet_Array[1]); // Write packet length to serial + + do{ + servoSerial->putc(Instruction_Packet_Array[Counter + 2]); // Write Instuction & Parameters (if there is any) to serial + Counter++; + }while((Instruction_Packet_Array[1] - 2) >= Counter); + + servoSerial->putc(Instruction_Packet_Array[Counter + 2]); // Write check sum to serial + wait_us((Counter + 4)*11); + servoSerialDir->write(0); // Set TX Buffer pin to LOW after data has been sent + +} + + +unsigned int DynamixelClass::readStatusPacket(void){ + + unsigned char Counter = 0x00; + + static unsigned char InBuff[20]; + unsigned char i, j, RxState; + + + Status_Packet_Array[0] = 0x00; + Status_Packet_Array[1] = 0x00; + Status_Packet_Array[2] = 0x00; + Status_Packet_Array[3] = 0x00; + i=0; RxState=0; j=0; InBuff[0]=0; + Timer timer; + timer.start(); + + +while (RxState<3){ // Wait for " header + header + frame length + error " RX data + if (timer.read_ms() >= STATUS_PACKET_TIMEOUT){ + return Status_Packet_Array[2] = 0x80; // Return with Error if Serial data not received with in time limit + + } + + if (servoSerial->readable()) { + InBuff[i]=servoSerial->getc(); + if (InBuff[0]==0xff) i++; // When we have the first header we starts to inc data to inbuffer + + + if ((i>=(STATUS_FRAME_BUFFER-1)) &&(RxState==0)) RxState++; //read header + + switch (RxState) { + case 1: {//Read header + if ((InBuff[j] == 0xFF) && (InBuff[j+1]== 0xFF)){ // checkes that we have got the buffer + j=2; + Status_Packet_Array[0] = InBuff[j++]; // ID sent from Dynamixel + Status_Packet_Array[1] = InBuff[j++]; // Frame Length of status packet + Status_Packet_Array[2] = InBuff[j++]; // Error (char) + RxState++; led2->write(0); + } + } break; + case 2: {//Read data + if (i>Status_Packet_Array[1]+3) { //We have recieved all data + do{ + Status_Packet_Array[3 + Counter] = InBuff[j++]; + Counter++; + }while(Status_Packet_Array[1] > Counter); // Read Parameter(s) into array + + Status_Packet_Array[Counter + 4] = InBuff[j++]; // Read Check sum + RxState++; + } + } break; + } //switch + } + }//while.. + timer.stop(); +// debugStatusframe(); + + return 0x00; +} + + + +//------------------------------------------------------------------------------------------------------------------------------- +// Public Methods +//------------------------------------------------------------------------------------------------------------------------------- + + DynamixelClass::DynamixelClass(int baud, PinName D_Pin, PinName tx, PinName rx){ + servoSerial=new Serial(tx, rx); + servoSerial->baud(baud); + servoSerialDir= new DigitalOut(D_Pin); + servoSerialDir->write(0); + led2=new DigitalOut(LED2); + +} + + DynamixelClass::~DynamixelClass(){ + if(servoSerial != NULL) + delete servoSerial; +} + +unsigned int DynamixelClass::reset(unsigned char ID){ + + Instruction_Packet_Array[0] = ID; + Instruction_Packet_Array[1] = RESET_LENGTH; + Instruction_Packet_Array[2] = COMMAND_RESET; + Instruction_Packet_Array[3] = ~(ID + RESET_LENGTH + COMMAND_RESET); //Checksum; + + if (servoSerial->readable()) + while (servoSerial->readable()) servoSerial->getc(); //emthy buffer + + transmitInstructionPacket(); + + if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it + return (0x00); + }else{ + readStatusPacket(); + if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value + return (Status_Packet_Array[0]); // Return SERVO ID + }else{ + return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value + } + } +} + +void DynamixelClass::NewBaudRate(int baud) { +//Change the baudrate on then MBED + int Baudrate_BPS = 0; + Baudrate_BPS =(int) 2000000 / (baud + 1); // Calculate Baudrate as ber "Robotis e-manual" + servoSerial->baud(Baudrate_BPS); +} + +unsigned int DynamixelClass::ping(unsigned char ID){ + + Instruction_Packet_Array[0] = ID; + Instruction_Packet_Array[1] = PING_LENGTH; + Instruction_Packet_Array[2] = COMMAND_PING; + Instruction_Packet_Array[3] = ~(ID + PING_LENGTH + COMMAND_PING); + + if (servoSerial->readable()) + while (servoSerial->readable()) servoSerial->getc(); //emthy buffer + + transmitInstructionPacket(); + readStatusPacket(); + + if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value + return (Status_Packet_Array[0]); // Return SERVO ID + }else{ + return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value + } +} + +unsigned int DynamixelClass::setStatusPaketReturnDelay(unsigned char ID,unsigned char ReturnDelay){ + + Instruction_Packet_Array[0] = ID; + Instruction_Packet_Array[1] = SET_RETURN_LENGTH; + Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; + Instruction_Packet_Array[3] = EEPROM_RETURN_DELAY_TIME; + Instruction_Packet_Array[4] = (char) (ReturnDelay/2); + Instruction_Packet_Array[5] = ~(ID + SET_RETURN_LENGTH + COMMAND_WRITE_DATA + EEPROM_RETURN_DELAY_TIME + (char)(ReturnDelay/2)); + + if (servoSerial->readable()) + while (servoSerial->readable()) servoSerial->getc(); //emthy buffer + + transmitInstructionPacket(); + + if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it + return (0x00); + }else{ + readStatusPacket(); + if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value + return (Status_Packet_Array[0]); // Return SERVO ID + }else{ + return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value + } + } + +} + +unsigned int DynamixelClass::setID(unsigned char ID, unsigned char New_ID){ + + Instruction_Packet_Array[0] = ID; + Instruction_Packet_Array[1] = SET_ID_LENGTH; + Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; + Instruction_Packet_Array[3] = EEPROM_ID; + Instruction_Packet_Array[4] = New_ID; + Instruction_Packet_Array[5] = ~(ID + SET_ID_LENGTH + COMMAND_WRITE_DATA+ EEPROM_ID + New_ID); + + if (servoSerial->readable()) + while (servoSerial->readable()) servoSerial->getc(); //emthy buffer + + transmitInstructionPacket(); + + if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it + return (0x00); + }else{ + readStatusPacket(); + if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value + return (Status_Packet_Array[0]); // Return SERVO ID + }else{ + return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value + } + } +} + +unsigned int DynamixelClass::setBaudRate(unsigned char ID, long Baud){ + + Instruction_Packet_Array[0] = ID; + Instruction_Packet_Array[1] = SET_BD_LENGTH; + Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; + Instruction_Packet_Array[3] = EEPROM_BAUD_RATE; +// if (Baud > 2250000){ +if (Baud >= 1000000){ + switch (Baud){ + case 2250000: + Instruction_Packet_Array[4] = 0xFA; + break; + case 2500000: + Instruction_Packet_Array[4] = 0xFB; + break; + case 3000000: + Instruction_Packet_Array[4] = 0xFC; + break; + case 1000000: + Instruction_Packet_Array[4] = 0x01; + } + }else{ + Instruction_Packet_Array[4] = (char)((2000000/Baud) - 1); + } + Instruction_Packet_Array[5] = ~(ID + SET_BD_LENGTH + COMMAND_WRITE_DATA + EEPROM_BAUD_RATE + (char)((2000000/Baud) - 1) ); + + if (servoSerial->readable()) + while (servoSerial->readable()) servoSerial->getc(); //emthy buffer + + transmitInstructionPacket(); + + if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it + return (0x00); + }else{ + readStatusPacket(); + if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value + return (Status_Packet_Array[0]); // Return SERVO ID + }else{ + return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value + } + } +} + +unsigned int DynamixelClass::setMaxTorque( unsigned char ID, int Torque){ + + Instruction_Packet_Array[0] = ID; + Instruction_Packet_Array[1] = SET_MAX_TORQUE_LENGTH; + Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; + Instruction_Packet_Array[3] = EEPROM_MAX_TORQUE_L ; + Instruction_Packet_Array[4] = (char)(Torque); + Instruction_Packet_Array[5] = (char)(Torque >> 8); + Instruction_Packet_Array[6] = ~(ID + SET_MAX_TORQUE_LENGTH + COMMAND_WRITE_DATA + EEPROM_MAX_TORQUE_L + (char)(Torque) + (char)(Torque >> 8)); + + if (servoSerial->readable()) + while (servoSerial->readable()) servoSerial->getc(); //emthy buffer + + transmitInstructionPacket(); + + if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it + return (0x00); + }else{ + readStatusPacket(); + if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value + return (Status_Packet_Array[0]); // Return SERVO ID + }else{ + return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value + } + } +} + +unsigned int DynamixelClass::setHoldingTorque(unsigned char ID, bool Set){ + + Instruction_Packet_Array[0] = ID; + Instruction_Packet_Array[1] = SET_HOLDING_TORQUE_LENGTH; + Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; + Instruction_Packet_Array[3] = RAM_TORQUE_ENABLE; + Instruction_Packet_Array[4] = Set; + Instruction_Packet_Array[5] = ~(ID + SET_HOLDING_TORQUE_LENGTH + COMMAND_WRITE_DATA + RAM_TORQUE_ENABLE + Set); + + if (servoSerial->readable()) + while (servoSerial->readable()) servoSerial->getc(); //emthy buffer + + transmitInstructionPacket(); + + if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it + return (0x00); + }else{ + readStatusPacket(); + if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value + return (Status_Packet_Array[0]); // Return SERVO ID + }else{ + return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value + } + } +} + +unsigned int DynamixelClass::setAlarmShutdown(unsigned char ID,unsigned char Set){ + + Instruction_Packet_Array[0] = ID; + Instruction_Packet_Array[1] = SET_ALARM_LENGTH; + Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; + Instruction_Packet_Array[3] = EEPROM_ALARM_SHUTDOWN; + Instruction_Packet_Array[4] = Set; + Instruction_Packet_Array[5] = ~(ID + SET_ALARM_LENGTH + COMMAND_WRITE_DATA + EEPROM_ALARM_SHUTDOWN + Set); + + if (servoSerial->readable()) + while (servoSerial->readable()) servoSerial->getc(); //emthy buffer + + transmitInstructionPacket(); + + if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it + return (0x00); + }else{ + readStatusPacket(); + if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value + return (Status_Packet_Array[0]); // Return SERVO ID + }else{ + return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value + } + } + +} + +unsigned int DynamixelClass::setStatusPaket(unsigned char ID,unsigned char Set){ + + Instruction_Packet_Array[0] = ID; + Instruction_Packet_Array[1] = SET_RETURN_LEVEL_LENGTH; + Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; + Instruction_Packet_Array[3] = EEPROM_RETURN_LEVEL; + Instruction_Packet_Array[4] = Set; + Instruction_Packet_Array[5] = ~(ID + SET_RETURN_LEVEL_LENGTH + COMMAND_WRITE_DATA + EEPROM_RETURN_LEVEL + Set); + + Status_Return_Value = Set; + + if (servoSerial->readable()) + while (servoSerial->readable()) servoSerial->getc(); //emthy buffer + + transmitInstructionPacket(); + + if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it + return (0x00); + }else{ + readStatusPacket(); + if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value + return (Status_Packet_Array[0]); // Return SERVO ID + }else{ + return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value + } + } + +} + + +unsigned int DynamixelClass::setMode(unsigned char ID, bool Dynamixel_Mode, unsigned int Dynamixel_CW_Limit,unsigned int Dynamixel_CCW_Limit){ + + Instruction_Packet_Array[0] = ID; + Instruction_Packet_Array[1] = SET_MODE_LENGTH; + Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; + Instruction_Packet_Array[3] = EEPROM_CW_ANGLE_LIMIT_L; + if ( Dynamixel_Mode == WHEEL) { // Set WHEEL mode, this is done by setting both the clockwise and anti-clockwise angle limits to ZERO + Instruction_Packet_Array[4] = 0x00; + Instruction_Packet_Array[5] = 0x00; + Instruction_Packet_Array[6] = 0x00; + Instruction_Packet_Array[7] = 0x00; + Instruction_Packet_Array[8] = ~(ID + SET_MODE_LENGTH + COMMAND_WRITE_DATA + EEPROM_CW_ANGLE_LIMIT_L); + }else { // Else set SERVO mode + Instruction_Packet_Array[4] = (char)(Dynamixel_CW_Limit); + Instruction_Packet_Array[5] = (char)((Dynamixel_CW_Limit & 0x0F00) >> 8); + Instruction_Packet_Array[6] = (char)(Dynamixel_CCW_Limit); + Instruction_Packet_Array[7] = (char)((Dynamixel_CCW_Limit & 0x0F00) >> 8); + Instruction_Packet_Array[8] = ~(ID + SET_MODE_LENGTH + COMMAND_WRITE_DATA + EEPROM_CW_ANGLE_LIMIT_L + (char)(Dynamixel_CW_Limit) + (char)((Dynamixel_CW_Limit & 0x0F00) >> 8) + (char)(Dynamixel_CCW_Limit) + (char)((Dynamixel_CCW_Limit & 0x0F00) >> 8)); + } + + + if (servoSerial->readable()) + while (servoSerial->readable()) servoSerial->getc(); //emthy buffer + + transmitInstructionPacket(); + + if (Status_Return_Value == ALL){ + readStatusPacket(); + if (Status_Packet_Array[2] != 0){ + return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value + } + + } + return 0x00; //if no errors + } + + unsigned int DynamixelClass::setPunch(unsigned char ID,unsigned int Punch){ + + Instruction_Packet_Array[0] = ID; + Instruction_Packet_Array[1] = SET_PUNCH_LENGTH; + Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; + Instruction_Packet_Array[3] = RAM_PUNCH_L; + Instruction_Packet_Array[4] = (char)(Punch); + Instruction_Packet_Array[5] = (char)(Punch >> 8); + Instruction_Packet_Array[6] = ~(ID + SET_PUNCH_LENGTH + COMMAND_WRITE_DATA + RAM_PUNCH_L + (char)(Punch) + (char)(Punch >> 8) ); + + if (servoSerial->readable()) + while (servoSerial->readable()) servoSerial->getc(); //emthy buffer + + transmitInstructionPacket(); + + if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it + return (0x00); + }else{ + readStatusPacket(); + if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value + return (Status_Packet_Array[0]); // Return SERVO ID + }else{ + return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value + } + } + + } + + unsigned int DynamixelClass::setPID(unsigned char ID ,unsigned char P,unsigned char I,unsigned char D){ + + Instruction_Packet_Array[0] = ID; + Instruction_Packet_Array[1] = SET_PID_LENGTH; + Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; + Instruction_Packet_Array[3] = RAM_PROPORTIONAL_GAIN; + Instruction_Packet_Array[4] = P; + Instruction_Packet_Array[5] = I; + Instruction_Packet_Array[6] = D; + Instruction_Packet_Array[7] = ~(ID + SET_PID_LENGTH + COMMAND_WRITE_DATA + RAM_PROPORTIONAL_GAIN + P + I + D ); + + if (servoSerial->readable()) + while (servoSerial->readable()) servoSerial->getc(); //emthy buffer + + transmitInstructionPacket(); + + if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it + return (0x00); + }else{ + readStatusPacket(); + if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value + return (Status_Packet_Array[0]); // Return SERVO ID + }else{ + return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value + } + } + } + +unsigned int DynamixelClass::setTemp(unsigned char ID,unsigned char temp){ + + Instruction_Packet_Array[0] = ID; + Instruction_Packet_Array[1] = SET_TEMP_LENGTH; + Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; + Instruction_Packet_Array[3] = EEPROM_LIMIT_TEMPERATURE; + Instruction_Packet_Array[4] = temp; + Instruction_Packet_Array[5] = ~(ID + SET_TEMP_LENGTH + COMMAND_WRITE_DATA + EEPROM_LIMIT_TEMPERATURE + temp); + + if (servoSerial->readable()) + while (servoSerial->readable()) servoSerial->getc(); //emthy buffer + + transmitInstructionPacket(); + + if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it + return (0x00); + }else{ + readStatusPacket(); + if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value + return (Status_Packet_Array[0]); // Return SERVO ID + }else{ + return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value + } + } +} + +unsigned int DynamixelClass::setVoltage(unsigned char ID,unsigned char Volt_L, unsigned char Volt_H){ + + Instruction_Packet_Array[0] = ID; + Instruction_Packet_Array[1] = SET_VOLT_LENGTH; + Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; + Instruction_Packet_Array[3] = EEPROM_LOW_LIMIT_VOLTAGE; + Instruction_Packet_Array[4] = Volt_L; + Instruction_Packet_Array[5] = Volt_H; + Instruction_Packet_Array[6] = ~(ID + SET_VOLT_LENGTH + COMMAND_WRITE_DATA + EEPROM_LOW_LIMIT_VOLTAGE + Volt_L + Volt_H); + + if (servoSerial->readable()) + while (servoSerial->readable()) servoSerial->getc(); //emthy buffer + + transmitInstructionPacket(); + + if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it + return (0x00); + }else{ + readStatusPacket(); + if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value + return (Status_Packet_Array[0]); // Return SERVO ID + }else{ + return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value + } + } +} + +unsigned int DynamixelClass::servo(unsigned char ID,unsigned int Position,unsigned int Speed){ + + Instruction_Packet_Array[0] = ID; + Instruction_Packet_Array[1] = SERVO_GOAL_LENGTH; + Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; + Instruction_Packet_Array[3] = RAM_GOAL_POSITION_L; + Instruction_Packet_Array[4] = (char)(Position); + Instruction_Packet_Array[5] = (char)((Position & 0x0F00) >> 8); + Instruction_Packet_Array[6] = (char)(Speed); + Instruction_Packet_Array[7] = (char)((Speed & 0x0F00) >> 8); + Instruction_Packet_Array[8] = ~(ID + SERVO_GOAL_LENGTH + COMMAND_WRITE_DATA + RAM_GOAL_POSITION_L + Position + (char)((Position & 0x0F00) >> 8) + Speed + (char)((Speed & 0x0F00) >> 8)); + + if (servoSerial->readable()) + while (servoSerial->readable()) servoSerial->getc(); //emthy buffer + + transmitInstructionPacket(); + + + if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it + return (0x00); + }else{ + readStatusPacket(); + if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value + return (Status_Packet_Array[0]); // Return SERVO ID + }else{ + return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value + } + } +} + +unsigned int DynamixelClass::servoPreload(unsigned char ID,unsigned int Position,unsigned int Speed){ + + Instruction_Packet_Array[0] = ID; + Instruction_Packet_Array[1] = SERVO_GOAL_LENGTH; + Instruction_Packet_Array[2] = COMMAND_REG_WRITE_DATA; + Instruction_Packet_Array[3] = RAM_GOAL_POSITION_L; + Instruction_Packet_Array[4] = (char)(Position); + Instruction_Packet_Array[5] = (char)(Position >> 8); + Instruction_Packet_Array[6] = (char)(Speed); + Instruction_Packet_Array[7] = (char)(Speed >> 8); + Instruction_Packet_Array[8] = ~(ID + SERVO_GOAL_LENGTH + COMMAND_REG_WRITE_DATA + RAM_GOAL_POSITION_L + (char)(Position) + (char)(Position >> 8) + (char)(Speed) + (char)(Speed >> 8)); + + if (servoSerial->readable()) + while (servoSerial->readable()) servoSerial->getc(); //emthy buffer + + transmitInstructionPacket(); + + if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it + return (0x00); + }else{ + readStatusPacket(); + if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value + return (Status_Packet_Array[0]); // Return SERVO ID + }else{ + return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value + } + } +} + +unsigned int DynamixelClass::wheel(unsigned char ID, bool Rotation,unsigned int Speed){ + + char Speed_H,Speed_L; + Speed_L = Speed; + if (Rotation == 0){ // Move Left + Speed_H = Speed >> 8; + } + else if (Rotation == 1){ // Move Right + Speed_H = (Speed >> 8)+4; + } + + Instruction_Packet_Array[0] = ID; + Instruction_Packet_Array[1] = WHEEL_LENGTH; + Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; + Instruction_Packet_Array[3] = RAM_GOAL_SPEED_L; + Instruction_Packet_Array[4] = Speed_L; + Instruction_Packet_Array[5] = Speed_H; + Instruction_Packet_Array[6] = ~(ID + WHEEL_LENGTH + COMMAND_WRITE_DATA + RAM_GOAL_SPEED_L + Speed_L + Speed_H); + + if (servoSerial->readable()) + while (servoSerial->readable()) servoSerial->getc(); //emthy buffer + + transmitInstructionPacket(); + + if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it + return (0x00); + }else{ + readStatusPacket(); + if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value + return (Status_Packet_Array[0]); // Return SERVO ID + }else{ + return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value + } + } + +} + +void DynamixelClass::wheelSync(unsigned char ID1, bool Dir1, unsigned int Speed1, unsigned char ID2, bool Dir2, unsigned int Speed2, unsigned char ID3, bool Dir3, unsigned int Speed3){ + + char Speed1_H,Speed1_L; + Speed1_L = Speed1; + if (Dir1 == 0){ // Move Left + Speed1_H = Speed1 >> 8; + } + else if (Dir1 == 1) // Move Right + { + Speed1_H = (Speed1 >> 8)+4; + } + + char Speed2_H,Speed2_L; + Speed2_L = Speed2; + if (Dir2 == 0){ // Move Left + Speed2_H = Speed2 >> 8; + } + else if (Dir2 == 1) // Move Right + { + Speed2_H = (Speed2 >> 8)+4; + } + + char Speed3_H,Speed3_L; + Speed3_L = Speed3; + if (Dir3 == 0){ // Move Left + Speed3_H = Speed3 >> 8; + } + else if (Dir3 == 1) // Move Right + { + Speed3_H = (Speed3 >> 8)+4; + } + + Instruction_Packet_Array[0] = 0xFE; // When Writing a Sync comman you must address all(0xFE) servos + Instruction_Packet_Array[1] = SYNC_LOAD_LENGTH; + Instruction_Packet_Array[2] = COMMAND_SYNC_WRITE; + Instruction_Packet_Array[3] = RAM_GOAL_SPEED_L; + Instruction_Packet_Array[4] = SYNC_DATA_LENGTH; + Instruction_Packet_Array[5] = ID1; + Instruction_Packet_Array[6] = Speed1_L; + Instruction_Packet_Array[7] = Speed1_H; + Instruction_Packet_Array[8] = ID2; + Instruction_Packet_Array[9] = Speed2_L; + Instruction_Packet_Array[10] = Speed2_H; + Instruction_Packet_Array[11] = ID3; + Instruction_Packet_Array[12] = Speed3_L; + Instruction_Packet_Array[13] = Speed3_H; + Instruction_Packet_Array[14] = (char)(~(0xFE + SYNC_LOAD_LENGTH + COMMAND_SYNC_WRITE + RAM_GOAL_SPEED_L + SYNC_DATA_LENGTH + ID1 + Speed1_L + Speed1_H + ID2 + Speed2_L + Speed2_H + ID3 + Speed3_L + Speed3_H)); + + transmitInstructionPacket(); + +} + +unsigned int DynamixelClass::wheelPreload(unsigned char ID, bool Dir,unsigned int Speed){ + + char Speed_H,Speed_L; + Speed_L = Speed; + if (Dir == 0){ // Move Left + Speed_H = Speed >> 8; + } + else if (Dir == 1) // Move Right + { + Speed_H = (Speed >> 8)+4; + } + + Instruction_Packet_Array[0] = ID; + Instruction_Packet_Array[1] = WHEEL_LENGTH; + Instruction_Packet_Array[2] = COMMAND_REG_WRITE_DATA; + Instruction_Packet_Array[3] = RAM_GOAL_SPEED_L; + Instruction_Packet_Array[4] = Speed_L; + Instruction_Packet_Array[5] = Speed_H; + Instruction_Packet_Array[6] = ~(ID + WHEEL_LENGTH + COMMAND_REG_WRITE_DATA + RAM_GOAL_SPEED_L + Speed_L + Speed_H); + + if (servoSerial->readable()) + while (servoSerial->readable()) servoSerial->getc(); //emthy buffer + + transmitInstructionPacket(); + + if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it + return (0x00); + }else{ + readStatusPacket(); + if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value + return (Status_Packet_Array[0]); // Return SERVO ID + }else{ + return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value + } + } + +} + +unsigned int DynamixelClass::action(unsigned char ID){ + + Instruction_Packet_Array[0] = ID; + Instruction_Packet_Array[1] = RESET_LENGTH; + Instruction_Packet_Array[2] = COMMAND_ACTION; + Instruction_Packet_Array[3] = ~(ID + ACTION_LENGTH + COMMAND_ACTION); + + if (servoSerial->readable()) + while (servoSerial->readable()) servoSerial->getc(); //emthy buffer + + transmitInstructionPacket(); + + if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it + return (0x00); + }else{ + readStatusPacket(); + if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value + return (Status_Packet_Array[0]); // Return SERVO ID + }else{ + return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value + } + } +} + +unsigned int DynamixelClass::ledState(unsigned char ID, bool Status){ + + Instruction_Packet_Array[0] = ID; + Instruction_Packet_Array[1] = LED_LENGTH; + Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; + Instruction_Packet_Array[3] = RAM_LED; + Instruction_Packet_Array[4] = Status; + Instruction_Packet_Array[5] = ~(ID + LED_LENGTH + COMMAND_WRITE_DATA + RAM_LED + Status); + + if (servoSerial->readable()) + while (servoSerial->readable()) servoSerial->getc(); //emthy buffer + + transmitInstructionPacket(); + + if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it + return (0x00); + }else{ + readStatusPacket(); + if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value + return (Status_Packet_Array[0]); // Return SERVO ID + }else{ + return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value + } + } +} + +unsigned int DynamixelClass::readTemperature(unsigned char ID){ + + Instruction_Packet_Array[0] = ID; + Instruction_Packet_Array[1] = READ_TEMP_LENGTH; + Instruction_Packet_Array[2] = COMMAND_READ_DATA; + Instruction_Packet_Array[3] = RAM_PRESENT_TEMPERATURE; + Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH; + Instruction_Packet_Array[5] = ~(ID + READ_TEMP_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_TEMPERATURE + READ_ONE_BYTE_LENGTH); + + if (servoSerial->readable()) + while (servoSerial->readable()) servoSerial->getc(); //emthy buffer + + transmitInstructionPacket(); + readStatusPacket(); + + if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value + return Status_Packet_Array[3]; + }else{ + return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value + } +} + +unsigned int DynamixelClass::readPosition(unsigned char ID){ + + Instruction_Packet_Array[0] = ID; + Instruction_Packet_Array[1] = READ_POS_LENGTH; + Instruction_Packet_Array[2] = COMMAND_READ_DATA; + Instruction_Packet_Array[3] = RAM_PRESENT_POSITION_L; + Instruction_Packet_Array[4] = READ_TWO_BYTE_LENGTH; + Instruction_Packet_Array[5] = ~(ID + READ_POS_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_POSITION_L + READ_TWO_BYTE_LENGTH); + + if (servoSerial->readable()) + while (servoSerial->readable()) servoSerial->getc(); //emthy buffer + + transmitInstructionPacket(); + readStatusPacket(); + + if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value // If there is no status packet error return value + return Status_Packet_Array[4] << 8 | Status_Packet_Array[3]; // Return present position value + }else{ + return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value + } +} + +unsigned int DynamixelClass::readLoad(unsigned char ID){ + + Instruction_Packet_Array[0] = ID; + Instruction_Packet_Array[1] = READ_LOAD_LENGTH; + Instruction_Packet_Array[2] = COMMAND_READ_DATA; + Instruction_Packet_Array[3] = RAM_PRESENT_LOAD_L; + Instruction_Packet_Array[4] = READ_TWO_BYTE_LENGTH; + Instruction_Packet_Array[5] = ~(ID + READ_LOAD_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_LOAD_L + READ_TWO_BYTE_LENGTH); + + if (servoSerial->readable()) + while (servoSerial->readable()) servoSerial->getc(); //emthy buffer + + transmitInstructionPacket(); + readStatusPacket(); + + if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value + return ((Status_Packet_Array[4] << 8) | Status_Packet_Array[3]); // Return present load value + }else{ + return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value + } +} + +unsigned int DynamixelClass::readSpeed(unsigned char ID){ + + Instruction_Packet_Array[0] = ID; + Instruction_Packet_Array[1] = READ_SPEED_LENGTH; + Instruction_Packet_Array[2] = COMMAND_READ_DATA; + Instruction_Packet_Array[3] = RAM_PRESENT_SPEED_L; + Instruction_Packet_Array[4] = READ_TWO_BYTE_LENGTH; + Instruction_Packet_Array[5] = ~(ID + READ_SPEED_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_SPEED_L + READ_TWO_BYTE_LENGTH); + + if (servoSerial->readable()) + while (servoSerial->readable()) servoSerial->getc(); //emthy buffer + + transmitInstructionPacket(); + readStatusPacket(); + + if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value + return (Status_Packet_Array[4] << 8) | Status_Packet_Array[3]; // Return present position value + }else{ + return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value + } +} + + +unsigned int DynamixelClass::readVoltage(unsigned char ID){ + + Instruction_Packet_Array[0] = ID; + Instruction_Packet_Array[1] = READ_VOLT_LENGTH; + Instruction_Packet_Array[2] = COMMAND_READ_DATA; + Instruction_Packet_Array[3] = RAM_PRESENT_VOLTAGE; + Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH; + Instruction_Packet_Array[5] = ~(ID + READ_VOLT_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_VOLTAGE + READ_ONE_BYTE_LENGTH); + + if (servoSerial->readable()) + while (servoSerial->readable()) servoSerial->getc(); //emthy buffer + + transmitInstructionPacket(); + readStatusPacket(); + + if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value + return Status_Packet_Array[3]; // Return voltage value (value retured by Dynamixel is 10 times actual voltage) + }else{ + return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value + } +} + +unsigned int DynamixelClass::checkRegister(unsigned char ID){ + + Instruction_Packet_Array[0] = ID; + Instruction_Packet_Array[1] = READ_REGISTER_LENGTH; + Instruction_Packet_Array[2] = COMMAND_READ_DATA; + Instruction_Packet_Array[3] = RAM_REGISTER; + Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH; + Instruction_Packet_Array[5] = ~(ID + READ_REGISTER_LENGTH + COMMAND_READ_DATA + RAM_REGISTER + READ_ONE_BYTE_LENGTH); + + if (servoSerial->readable()) + while (servoSerial->readable()) servoSerial->getc(); //emthy buffer + + transmitInstructionPacket(); + readStatusPacket(); + + if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value + return (Status_Packet_Array[3]); // Return register value + }else{ + return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value + } +} + +unsigned int DynamixelClass::checkMovement(unsigned char ID){ + + Instruction_Packet_Array[0] = ID; + Instruction_Packet_Array[1] = READ_MOVING_LENGTH; + Instruction_Packet_Array[2] = COMMAND_READ_DATA; + Instruction_Packet_Array[3] = RAM_MOVING; + Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH; + Instruction_Packet_Array[5] = ~(ID + READ_MOVING_LENGTH + COMMAND_READ_DATA + RAM_MOVING + READ_ONE_BYTE_LENGTH); + + if (servoSerial->readable()) + while (servoSerial->readable()) servoSerial->getc(); //emthy buffer + + transmitInstructionPacket(); + readStatusPacket(); + + if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value + return (Status_Packet_Array[3]); // Return movement value + }else{ + return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value + } +} + +unsigned int DynamixelClass::checkLock(unsigned char ID){ + + Instruction_Packet_Array[0] = ID; + Instruction_Packet_Array[1] = READ_LOCK_LENGTH; + Instruction_Packet_Array[2] = COMMAND_READ_DATA; + Instruction_Packet_Array[3] = RAM_LOCK; + Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH; + Instruction_Packet_Array[5] = ~(ID + READ_LOCK_LENGTH + COMMAND_READ_DATA + RAM_LOCK + READ_ONE_BYTE_LENGTH); + + if (servoSerial->readable()) + while (servoSerial->readable()) servoSerial->getc(); //emthy buffer + + transmitInstructionPacket(); + readStatusPacket(); + + if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value + return (Status_Packet_Array[3]); // Return Lock value + }else{ + return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value + } +} + +unsigned int DynamixelClass::torqueMode(unsigned char ID, bool Status){ + + Instruction_Packet_Array[0] = ID; + Instruction_Packet_Array[1] = 0x04; + Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; + Instruction_Packet_Array[3] = RAM_TORQUE_CONTROL_MODE_ENABLE; + Instruction_Packet_Array[4] = Status; + Instruction_Packet_Array[5] = ~(ID + 0x04 + COMMAND_WRITE_DATA + RAM_TORQUE_CONTROL_MODE_ENABLE + Status); + + if (servoSerial->readable()) + while (servoSerial->readable()) servoSerial->getc(); //emthy buffer + + transmitInstructionPacket(); + + if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it + return (0x00); + }else{ + readStatusPacket(); + if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value + return (Status_Packet_Array[0]); // Return SERVO ID + }else{ + return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value + } + } +} + +unsigned int DynamixelClass::torque(unsigned char ID,unsigned int Torque){ + + + Instruction_Packet_Array[0] = ID; + Instruction_Packet_Array[1] = 0x05; + Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; + Instruction_Packet_Array[3] = RAM_GOAL_TORQUE_L; + Instruction_Packet_Array[4] = (char)(Torque); + Instruction_Packet_Array[5] = (char)((Torque & 0x0F00) >> 8); + Instruction_Packet_Array[6] = ~(ID + 0x05 + COMMAND_WRITE_DATA + RAM_GOAL_TORQUE_L + (char)(Torque) + (char)((Torque & 0x0F00) >> 8) ); + + if (servoSerial->readable()) + while (servoSerial->readable()) servoSerial->getc(); //emthy buffer + + transmitInstructionPacket(); + + if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it + return (0x00); + }else{ + readStatusPacket(); + if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value + return (Status_Packet_Array[0]); // Return SERVO ID + }else{ + return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value + } + } + +} + +unsigned int DynamixelClass::readRegister(unsigned char ID,unsigned char Register){ + + Instruction_Packet_Array[0] = ID; + Instruction_Packet_Array[1] = 0x04; + Instruction_Packet_Array[2] = COMMAND_READ_DATA; + Instruction_Packet_Array[3] = Register; + Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH; + Instruction_Packet_Array[5] = ~(ID + READ_TEMP_LENGTH + COMMAND_READ_DATA + Register + READ_ONE_BYTE_LENGTH); + + if (servoSerial->readable()) + while (servoSerial->readable()) servoSerial->getc(); //emthy buffer + + transmitInstructionPacket(); + readStatusPacket(); + + if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value + return Status_Packet_Array[3]; + }else{ + return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value + } +} + +
diff -r 000000000000 -r bf9bf4b7625f Mx28.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Mx28.h Fri Jun 08 14:11:49 2018 +0000 @@ -0,0 +1,206 @@ +/* +How Dynamixel work can be found +-------------------------------- +Robotis e-Manual +http://support.robotis.com + +Overview of Communication +http://support.robotis.com/en/product/dynamixel/dxl_communication.htm + +Kind of Instruction +http://support.robotis.com/en/product/dynamixel/communication/dxl_instruction.htm + +Instruction Packet & Status Packet (Return Packet) +http://support.robotis.com/en/product/dynamixel/communication/dxl_packet.htm + +Control Table +http://support.robotis.com/en/product/dynamixel/mx_series/mx-28.htm + + */ + +#ifndef Mx28_h +#define Mx28_h + +//------------------------------------------------------------------------------------------------------------------------------- +// define - Dynamixel Hex code table +//------------------------------------------------------------------------------------------------------------------------------- +// EEPROM AREA +#define EEPROM_MODEL_NUMBER_L 0x00 +#define EEPROM_MODEL_NUMBER_H 0x01 +#define EEPROM_VERSION 0x02 +#define EEPROM_ID 0x03 +#define EEPROM_BAUD_RATE 0x04 +#define EEPROM_RETURN_DELAY_TIME 0x05 +#define EEPROM_CW_ANGLE_LIMIT_L 0x06 +#define EEPROM_CW_ANGLE_LIMIT_H 0x07 +#define EEPROM_CCW_ANGLE_LIMIT_L 0x08 +#define EEPROM_CCW_ANGLE_LIMIT_H 0x09 +#define EEPROM_LIMIT_TEMPERATURE 0x0B +#define EEPROM_LOW_LIMIT_VOLTAGE 0x0C +#define EEPROM_HIGN_LIMIT_VOLTAGE 0x0D +#define EEPROM_MAX_TORQUE_L 0x0E +#define EEPROM_MAX_TORQUE_H 0x0F +#define EEPROM_RETURN_LEVEL 0x10 +#define EEPROM_ALARM_LED 0x11 +#define EEPROM_ALARM_SHUTDOWN 0x12 +// RAM AREA +#define RAM_TORQUE_ENABLE 0x18 +#define RAM_LED 0x19 +#define RAM_PROPORTIONAL_GAIN 0x1A +#define RAM_INTERGRAL_GAIN 0x1B +#define RAM_DERIVATIVE_GAIN 0x1C +#define RAM_GOAL_POSITION_L 0x1E +#define RAM_GOAL_POSITION_H 0x1F +#define RAM_GOAL_SPEED_L 0x20 +#define RAM_GOAL_SPEED_H 0x21 +#define RAM_TORQUE_LIMIT_L 0x22 +#define RAM_TORQUE_LIMIT_H 0x23 +#define RAM_PRESENT_POSITION_L 0x24 +#define RAM_PRESENT_POSITION_H 0x25 +#define RAM_PRESENT_SPEED_L 0x26 +#define RAM_PRESENT_SPEED_H 0x27 +#define RAM_PRESENT_LOAD_L 0x28 +#define RAM_PRESENT_LOAD_H 0x29 +#define RAM_PRESENT_VOLTAGE 0x2A +#define RAM_PRESENT_TEMPERATURE 0x2B +#define RAM_REGISTER 0x2C +#define RAM_MOVING 0x2E +#define RAM_LOCK 0x2F +#define RAM_PUNCH_L 0x30 +#define RAM_PUNCH_H 0x31 + +#define RAM_TORQUE_CONTROL_MODE_ENABLE 0X46 +#define RAM_GOAL_TORQUE_L 0X47 +#define RAM_GOAL_TORQUE_H 0X48 + +//------------------------------------------------------------------------------------------------------------------------------- +// Instruction commands Set +//------------------------------------------------------------------------------------------------------------------------------- +#define COMMAND_PING 0x01 +#define COMMAND_READ_DATA 0x02 +#define COMMAND_WRITE_DATA 0x03 +#define COMMAND_REG_WRITE_DATA 0x04 +#define COMMAND_ACTION 0x05 +#define COMMAND_RESET 0x06 +#define COMMAND_SYNC_WRITE 0x83 + + +//------------------------------------------------------------------------------------------------------------------------------- +//Instruction packet lengths +#define READ_ONE_BYTE_LENGTH 0x01 +#define READ_TWO_BYTE_LENGTH 0x02 +#define RESET_LENGTH 0x02 +#define PING_LENGTH 0x02 +#define ACTION_LENGTH 0x02 +#define SET_ID_LENGTH 0x04 +#define SET_BD_LENGTH 0x04 +#define SET_RETURN_LEVEL_LENGTH 0x04 +#define READ_TEMP_LENGTH 0x04 +#define READ_POS_LENGTH 0x04 +#define READ_LOAD_LENGTH 0x04 +#define READ_SPEED_LENGTH 0x04 +#define READ_VOLT_LENGTH 0x04 +#define READ_REGISTER_LENGTH 0x04 +#define READ_MOVING_LENGTH 0x04 +#define READ_LOCK_LENGTH 0x04 +#define LED_LENGTH 0x04 +#define SET_HOLDING_TORQUE_LENGTH 0x04 +#define SET_MAX_TORQUE_LENGTH 0x05 +#define SET_ALARM_LENGTH 0x04 +#define READ_LOAD_LENGTH 0x04 +#define SET_RETURN_LENGTH 0x04 +#define WHEEL_LENGTH 0x05 +#define SERVO_GOAL_LENGTH 0x07 +#define SET_MODE_LENGTH 0x07 +#define SET_PUNCH_LENGTH 0x04 +#define SET_PID_LENGTH 0x06 +#define SET_TEMP_LENGTH 0x04 +#define SET_VOLT_LENGTH 0x05 +#define SYNC_LOAD_LENGTH 0x0D +#define SYNC_DATA_LENGTH 0x02 + + +//------------------------------------------------------------------------------------------------------------------------------- +// Specials +//------------------------------------------------------------------------------------------------------------------------------- + +#define OFF 0x00 +#define ON 0x01 + +#define SERVO 0x01 +#define WHEEL 0x00 + +#define LEFT 0x00 +#define RIGHT 0x01 + +#define NONE 0x00 +#define READ 0x01 +#define ALL 0x02 + +#define BROADCAST_ID 0xFE + +#define HEADER 0xFF + +#define STATUS_PACKET_TIMEOUT 50 // in millis() +#define STATUS_FRAME_BUFFER 5 + + + +class DynamixelClass { +private: + DigitalOut *servoSerialDir; + Serial *servoSerial; + void transmitInstructionPacket(void); + unsigned int readStatusPacket(void); + bool overflow; + void debugframe(void); + void debugStatusframe(void); + DigitalOut *led2; +public: + + DynamixelClass(int baud,PinName D_Pin,PinName tx, PinName rx); //Constructor + ~DynamixelClass(void); //destruktor + + void NewBaudRate(int baud); + unsigned int reset(unsigned char); + unsigned int ping(unsigned char); + + unsigned int setStatusPaketReturnDelay(unsigned char,unsigned char); + unsigned int setID(unsigned char, unsigned char); + unsigned int setBaudRate(unsigned char, long); + unsigned int setMaxTorque(unsigned char, int); + unsigned int setHoldingTorque(unsigned char, bool); + unsigned int setAlarmShutdown(unsigned char,unsigned char); + unsigned int setStatusPaket(unsigned char,unsigned char); + unsigned int setMode(unsigned char, bool, unsigned int, unsigned int); + unsigned int setPunch(unsigned char,unsigned int); + unsigned int setPID(unsigned char,unsigned char,unsigned char,unsigned char); + unsigned int setTemp(unsigned char,unsigned char); + unsigned int setVoltage(unsigned char,unsigned char,unsigned char); + + unsigned int servo(unsigned char, unsigned int, unsigned int); + unsigned int servoPreload(unsigned char, unsigned int, unsigned int); + unsigned int wheel(unsigned char, bool, unsigned int); + void wheelSync(unsigned char,bool,unsigned int,unsigned char, bool,unsigned int,unsigned char, bool,unsigned int); + unsigned int wheelPreload(unsigned char, bool, unsigned int); + + unsigned int action(unsigned char); + + unsigned int readTemperature(unsigned char); + unsigned int readVoltage(unsigned char); + unsigned int readPosition(unsigned char); + unsigned int readLoad(unsigned char); + unsigned int readSpeed(unsigned char); + + unsigned int checkRegister(unsigned char); + unsigned int checkMovement(unsigned char); + unsigned int checkLock(unsigned char); + + unsigned int ledState(unsigned char, bool); + + unsigned int torqueMode(unsigned char ID, bool Status); + unsigned int torque(unsigned char ID,unsigned int Torque); + unsigned int readRegister(unsigned char ID,unsigned char Register); +}; + +#endif
diff -r 000000000000 -r bf9bf4b7625f SensorFusion.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SensorFusion.cpp Fri Jun 08 14:11:49 2018 +0000 @@ -0,0 +1,113 @@ +#include "SensorFusion.h" +#include "SystemConstant.h" +#include "math.h" + +///Gyro and Acce data +float axm = 0.0f; +float aym = 0.0f; +float azm = 0.0f; +float u1 = 0.0f; +float u2 = 0.0f; +float u3 = 0.0f; +float mx = 0.0f; +float my = 0.0f; +float mz = 0.0f; +float Ac[3] = {0.0f, 0.0f, 0.0f}; + +float axm_f = 0.0f; +float axm_f_old = 0.0f; +float u3aym_f = 0.0f; +float u3aym_f_old = 0.0f; +float u2azm_f = 0.0f; +float u2azm_f_old = 0.0f; + +float aym_f = 0.0f; +float aym_f_old = 0.0f; +float u3axm_f = 0.0f; +float u3axm_f_old = 0.0f; +float u1azm_f = 0.0f; +float u1azm_f_old = 0.0f; + +float azm_f = 0.0f; +float azm_f_old = 0.0f; +float u2axm_f = 0.0f; +float u2axm_f_old = 0.0f; +float u1aym_f = 0.0f; +float u1aym_f_old = 0.0f; + +float x1_hat = 0.0f; +float x2_hat = 0.0f; +float x3_hat = 0.0f; +float sinroll = 0.0f; +float cosroll = 0.0f; +float roll_angle = 0.0f; +float roll_angle_old = 0.0f; +float droll_angle= 0.0f; +float droll_angle_old= 0.0f; +float sinpitch = 0.0f; +float cospitch = 0.0f; +float yaw_ref = 0.0f; +float yaw_angle = 0.0f; +float yaw_angle_old = 0.0f; +float dyaw_angle = 0.0f; +float dyaw_angle_old = 0.0f; +// *************************************************************************************************************************************************************************** +// Low-Pass Filter +float lpf(float input,float input_old,float frequency) +{ + float output = 0; + + //y[n] = a*y[n-1] + (1 - a)*x[n] + output = input*frequency*sample_time+input_old*(1-frequency*sample_time); + + return output; +}// Low-Pass Filter + +// *************************************************************************************************************************************************************************** +// x2_hat_estimat with finding roll angle +void roll_fusion(float a_xm,float a_ym,float a_zm,float u_3,float u_1,float alpha) +{ + // x2_hat estimation + aym_f = lpf(a_ym,aym_f_old,alpha); + aym_f_old = aym_f; + u3axm_f = lpf(u_3*a_xm,u3axm_f_old,alpha); // w3axm + u3axm_f_old = u3axm_f; + u1azm_f = lpf(u_1*a_zm,u1azm_f_old,alpha); // w1azm + u1azm_f_old = u1azm_f; + + x2_hat = -u3axm_f/alpha + aym_f + u1azm_f/alpha; //THe Low-Pass Filter(alpha/(s+alpha)) have already been done + + // Finding the roll angle and giving the limit for it + sinroll = x2_hat*(-0.1020); // This is for finding the roll angle, because x2_hat = -gsin(roll_angle). This is from paper Observer-Based Sensor Fusion. + if(sinroll >= 1.0f) + { + sinroll = 1.0; + cosroll = 0.0; + } + else if(sinroll <= -1.0f) + { + sinroll = -1.0; + cosroll = 0.0; + } + else cosroll = sqrt(1-(sinroll*sinroll)); + roll_angle = asin(sinroll);///By 312 Rotation +// roll_angle = lpf(roll_angle, roll_angle_old, 3); +// roll_angle_old = roll_angle; +}// roll_fusion + +// *************************************************************************************************************************************************************************** +// absolute +float absolute(float value) +{ + if(value < 0)return -value; + else return value; +}// absolute + +// *************************************************************************************************************************************************************************** +// Reset_data +void Reset_data(void) +{ + axm_f = axm_f_old = u3aym_f = u3aym_f_old = u2azm_f = u2azm_f_old = 0.0; + aym_f = aym_f_old = u3axm_f = u3axm_f_old = u1azm_f = u1azm_f_old = 0.0; + azm_f = azm_f_old = u2axm_f = u2axm_f_old = u1aym_f = u1aym_f_old = 0.0; +}// Reset_data \ No newline at end of file
diff -r 000000000000 -r bf9bf4b7625f SensorFusion.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SensorFusion.h Fri Jun 08 14:11:49 2018 +0000 @@ -0,0 +1,28 @@ +#ifndef SENSORFUSION_H_INCLUDED +#define SENSORFUSION_H_INCLUDED + +#include "math.h" +#define FTimer 1000.0f ///Hz +#define sample_time 1.0f/FTimer +#define Alpha FTimer/25.0f + +extern float axm, aym, azm, u1, u2, u3, mx, my, mz; +extern float Ac[3]; + +extern float axm_f, axm_f_old, u3aym_f, u3aym_f_old, u2azm_f, u2azm_f_old; +extern float aym_f, aym_f_old, u3axm_f, u3axm_f_old, u1azm_f, u1azm_f_old; +extern float azm_f, azm_f_old, u2axm_f, u2axm_f_old, u1aym_f, u1aym_f_old; + +extern float x1_hat, x2_hat, x3_hat; +extern float sinroll, cosroll, roll_angle, droll_angle, droll_angle_old; +extern float sinpitch, cospitch, yaw_ref, yaw_angle, yaw_angle_old, dyaw_angle, dyaw_angle_old; + +//extern void CentrifugalAcce_Compensation(float velocity); +extern float lpf(float input,float input_old,float frequency); +extern void roll_fusion(float a_xm,float a_ym,float a_zm,float u_3,float u_1,float alpha); +extern float Determinant(float x11, float x12, float x21, float x22); +extern float absolute(float value); + +extern void Reset_data(void); + +#endif // SENSORFUSION_H_INCLUDED \ No newline at end of file
diff -r 000000000000 -r bf9bf4b7625f SystemConstant.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SystemConstant.h Fri Jun 08 14:11:49 2018 +0000 @@ -0,0 +1,29 @@ +#ifndef SYSTEMCONST_H_INCLUDED +#define SYSTEMCONST_H_INCLUDED + +#define pi 3.14f + +#define M 23.1f +#define J 5.2067f +#define g 9.81f +#define Xt 0.3857f +#define Zt 0.4338f +#define L 1.053f +#define lammda 20/180*pi + +#define R_wheel 0.0695f ///radius of wheels +#define l_rs 0.095f ///length from rear wheel to sensor on XY plane +#define l_rg 0.13f ///length from rear wheel to center of mass on XY plane +#define Vmin 0.5f +#define Vmax 5.0f + + +/* + steer u2steer_lpf_freq*speed 1 + ----- = ----------------------------- * -------------------- + u S + u2steer_lpf_freq*speed u2steer_gain*speed +*/ +#define u2steer_lpf_freq 2.52668f//2.52668f +#define u2steer_gain 0.6251f + +#endif // ROBOTBICYCLECONST_H_INCLUDED
diff -r 000000000000 -r bf9bf4b7625f main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Jun 08 14:11:49 2018 +0000 @@ -0,0 +1,403 @@ +#include "mbed.h" +#include "math.h" +#include "LSM9DS0.h" +#include "Mx28.h" +#include "Controller.h" +#include "SensorFusion.h" +#include "SystemConstant.h" + +// Dynamixel ************************************************************************************************************************************************** +#define Steering_SERVO_ID 3 +#define SERVO_ControlPin A2 // Control pin of buffer chip, NOTE: this does not matter becasue we are not using a half to full contorl buffer. +#define SERVO_SET_Baudrate 1000000 // Baud rate speed which the Dynamixel will be set too (1Mbps) +#define TxPin A0 +#define RxPin A1 +#define CW_LIMIT_ANGLE 0x001 // lowest clockwise angle is 1, as when set to 0 it set servo to wheel mode +#define CCW_LIMIT_ANGLE 0xFFF // Highest anit-clockwise angle is 0XFFF, as when set to 0 it set servo to wheel mode +DynamixelClass dynamixelClass(SERVO_SET_Baudrate,SERVO_ControlPin,TxPin,RxPin); +float deg_s = 0.0; +float left_motor_angle = 0.0f; +float right_motor_angle = 0.0f; + +// LSM9DS0 ***************************************************************************************************************************************************** +// sensor gain +#define Gyro_gain_x 0.002422966362920f +#define Gyro_gain_y 0.002422966362920f +#define Gyro_gain_z 0.002422966362920f // datasheet : 70 mdeg/digit +#define Acce_gain_x -0.002403878; // -9.81 / ( 3317.81 - (-764.05) ) +#define Acce_gain_y -0.002375119; // -9.81 / ( 3476.34 - (-676.806)) +#define Acce_gain_z -0.002454702; // -9.81 / ( 4423.63 - (285.406) ) +#define Magn_gain 0 + +// raw data register +int16_t Gyro_axis_data[3] = {0}; // X, Y, Z axis +int16_t Acce_axis_data[3] = {0}; // X, Y, Z axis + +// sensor filter correction data +float Gyro_axis_data_f[3]; +float Gyro_axis_data_f_old[3]; +float Gyro_scale[3]; // scale = (data - zero) * gain +float Gyro_axis_zero[3] = {51.38514174, 62.64907136, -20.82209189};//{52.6302,57.3614,-48.6806}; +//Gyro offset +//********************************************** +//41.5376344 -26.92864125 103.3949169 +//42.53079179 -27.71065494 97.0400782 +//43.54056696 -28.63734115 90.77419355 +//********************************************** + + +float Acce_axis_data_f[3]; +float Acce_axis_data_f_old[3]; +float Acce_scale[3]; // scale = (data - zero) * gain +float Acce_axis_zero[3] = {-754.5894428, -677.0645161, 413.4472141};//{-818.8824, -714.3789, 326.2993};//{-721.0150,-748.3353,394.9194}; +//Acce offset +//********************************************** +//-618.8191593 -639.3255132 4676.384164 +//-604.7047898 3395.289345 375.0957967 +//4676.57478 -700.4506354 416.9599218 +//********************************************** +LSM9DS0 sensor(SPI_MODE, PB_2, PB_1); // PB_13, PB_14, PB_15 + +// Useful constants ******************************************************************************************************************************************* +#define T 0.001 //sampling time +#define CNT2STR 1500 // 1500(ms) = 1.5(s) +// mbed function *********************************************************************************************************************************************** +Ticker timer1; +Ticker timer2; + +Serial USB(USBTX, USBRX); +Serial BT_Cmd(PC_12, PD_2); +Serial BT_Data(PC_10, PC_11); +Serial MCU(PB_6, PA_10); + +// Linear actuator ********************************************************************************************************************************************* +PwmOut pwm_l(D7); +PwmOut pwmn_l(D11); + +PwmOut pwm_r(D8); +PwmOut pwmn_r(A3); + +#define down_duty 1.0f +#define up_duty 0.0f +#define stop_duty 0.5f + +// ADC +AnalogIn RightActuator(PC_0); +AnalogIn LeftActuator(PC_1); + +// Functions *************************************************************************************************************************************************** +void init_sensor(void); +void read_sensor(void); +void timer1_interrupt(void); +void timer2_interrupt(void); +void uart_send(void); +void separate(void); + +// Variables *************************************************************************************************************************************************** +int i = 0; +int display[6] = {0,0,0,0,0,0}; +char num[14] = {254,255,0,0,0,0,0,0,0,0,0,0,0,0}; // char num[0] , num[1] : 2 start byte +char BTCmd = 0; +char MCU_Data_get = 0; +char MCU_Data_put = 0; +float velocity = 0.0f; +float velocity_old = 0.0f; +float speed = 0.0f; +float VelocityCmd = 0.0f; +float VelocityCmdOld = 0.0f; +float steer_out = 0.0f; +float steer_out_old = 0.0f; +bool start_control = 0; +float roll_sum = 0.0f; +float average = 0.0f; +int temp = 0; +// turning command +int count2straight = 0; +float roll_cmd = 0.0f; +float roll_old = 0.0f; +float roll = 0.0f; + +float steer_cmd = 0.0f; +float steer_old = 0.0f; +float steer = 0.0f; +// test tool +bool up = 0; +bool down = 0; +bool tim1 = 0; +int counter = 0; + +// Code ******************************************************************************************************************************************************** +// main ******************************************************************************************************************************************************** +int main() +{ + dynamixelClass.setMode(Steering_SERVO_ID, SERVO, CW_LIMIT_ANGLE, CCW_LIMIT_ANGLE); // set mode to SERVO and set angle limits + USB.baud(115200); + BT_Data.baud(57600); + BT_Cmd.baud(115200); + MCU.baud(115200); + + // actuator pwm initialize + pwm_l.period_us(50); + pwm_r.period_us(50); + // timer setting + timer1.attach_us(&timer1_interrupt, 1000);//4ms interrupt period + timer2.attach_us(&timer2_interrupt, 3000);//4.098ms interrupt period + + // initialize + sensor.begin(); + start_control = 0; + steering_angle = 0; + + while(1) { + // command from tablet + if(BT_Cmd.readable()) { + BTCmd = BT_Cmd.getc(); + switch(BTCmd) { + case 's': + start_control = 1; + roll_cmd = 2.0f/180.0f*pi; + count2straight = 0; + break; + case 'p': + start_control = 0; + steer_out = 0; + steer_rad = 0; + steer_rad_old = 0; + steering_angle = 0.0f; + u_1 = 0; + u_2 = 0; + u_3 = 0; + u_d = 0; + u = 0; + alpha_1 = 0; + alpha_2 = 0; + roll_err = 0; + VelocityCmd = 0; + break; + case 'f': + steer_out = 0; + break; + case 'r': + steer_out = steer_out + 2; +// up = 1; +// down = 0; + break; + case 'l': + steer_out = steer_out - 2; +// up = 0; +// down = 1; + break; + case 'm': // command of turning right + roll_cmd = 4.0f/180.0f*pi; + count2straight = 0; + break; + case 'n': // command of turning left + roll_cmd = 0.0f/180.0f*pi; + count2straight = 0; + break; + case 'a': + VelocityCmd = VelocityCmd + 0.0f; + break; + case 'b': + VelocityCmd = VelocityCmd - 0.0f; + break; + case 'c': + break; + case 'h': + VelocityCmd = 0; + steer_out = 0; + break; + default: + break; + } + BTCmd = 0; + } + } +}// main + +// timer1_interrupt ********************************************************************************************************************************************* +void timer1_interrupt(void) +{ + // MCU_UART /////////////////////////////////////////////////////////////////////////////////////////////////////////////// + if(VelocityCmd != VelocityCmdOld) { + if(MCU.writeable()) { + MCU_Data_put = VelocityCmd / 0.01953125f; + MCU.putc(MCU_Data_put); + } + VelocityCmdOld = VelocityCmd; + } + // speed data from another MCU + if(MCU.readable()) + { + MCU_Data_get = MCU.getc(); + tim1=!tim1; + } + speed = (float)MCU_Data_get * 0.01953125f; + // velocity check for auxiliary wheels /////////////////////////////////////////////////////////////////////////////////// + if(speed < 0.3f){ + counter = 0; + pwm_l.write(down_duty); + pwm_r.write(down_duty); + } + if(speed > 0.3f){ + counter++; + pwm_l.write(up_duty); + pwm_r.write(up_duty); + if(counter >= 16000){ + counter = 16000; + pwm_l.write(stop_duty); + pwm_r.write(stop_duty); + } + } + // if(up ==1){ +// pwm_l.write(up_duty); +// pwm_r.write(up_duty); +// } +// if(down ==1){ +// pwm_l.write(down_duty); +// pwm_r.write(down_duty); +// } + + TIM1->CCER |= 0x4; + TIM1->CCER |= 0x40; + velocity = 1; + // IMU Read //////////////////////////////////////////////////////////////////////////////////////////////////////////////// + read_sensor(); + Acce_axis_data_f[0] = lpf(Acce_axis_data[0],Acce_axis_data_f_old[0],18); + Acce_axis_data_f_old[0] = Acce_axis_data_f[0]; + Acce_axis_data_f[1] = lpf(Acce_axis_data[1],Acce_axis_data_f_old[1],18); + Acce_axis_data_f_old[1] = Acce_axis_data_f[1]; + Acce_axis_data_f[2] = lpf(Acce_axis_data[2],Acce_axis_data_f_old[2],18); + Acce_axis_data_f_old[2] = Acce_axis_data_f[2]; + + Gyro_axis_data_f[0] = lpf(Gyro_axis_data[0],Gyro_axis_data_f_old[0],18); + Gyro_axis_data_f_old[0] = Gyro_axis_data_f[0]; + Gyro_axis_data_f[1] = lpf(Gyro_axis_data[1],Gyro_axis_data_f_old[1],18); + Gyro_axis_data_f_old[1] = Gyro_axis_data_f[1]; + Gyro_axis_data_f[2] = lpf(Gyro_axis_data[2],Gyro_axis_data_f_old[2],18); + Gyro_axis_data_f_old[2] = Gyro_axis_data_f[2]; + + Acce_scale[0] = ((Acce_axis_data_f[0]-Acce_axis_zero[0]))*Acce_gain_x; + Acce_scale[1] = ((Acce_axis_data_f[1]-Acce_axis_zero[1]))*Acce_gain_y; + Acce_scale[2] = ((Acce_axis_data_f[2]-Acce_axis_zero[2]))*Acce_gain_z; + + Gyro_scale[0] = ((Gyro_axis_data_f[0]-Gyro_axis_zero[0]))*Gyro_gain_x; + Gyro_scale[1] = ((Gyro_axis_data_f[1]-Gyro_axis_zero[1]))*Gyro_gain_y; + Gyro_scale[2] = ((Gyro_axis_data_f[2]-Gyro_axis_zero[2]))*Gyro_gain_z; + + //Centrifugal Acceleration Compensation //////////////////////////////////////////////////////////////////////////////////// + /* + Acx = Ac*sin(e) = YawRate*YawRate*l_rs + Acx = Ac*sin(e)*cos(roll_angle) = YawRate*speed*cos(roll_angle) + Acx = Ac*sin(e)*sin(roll_angle) = YawRate*speed*sin(roll_angle) + */ + Ac[0] = l_rs * Gyro_scale[2] * Gyro_scale[2]; + Ac[1] = (-1.0) * velocity * Gyro_scale[2] * cos(roll_angle); + Ac[2] = (-1.0) * velocity * Gyro_scale[2] * sin(roll_angle); + Acce_scale[0] = Acce_scale[0] - Ac[0]; + Acce_scale[1] = Acce_scale[1] - Ac[1]; + Acce_scale[2] = Acce_scale[2] - Ac[2]; + // do sensor fusion ///////////////////////////////////////////////////////////////////////////////////////////////////////// + roll_fusion(Acce_scale[0],Acce_scale[1],Acce_scale[2],Gyro_scale[2],Gyro_scale[0],5);// alpha = 3 represents the best behavior for robot bike + droll_angle = lpf(Gyro_scale[0], droll_angle_old, 62.8); // 62.8 = pi * 20 + droll_angle_old = droll_angle; + droll_angle = droll_angle + 0.5f/180.0f*pi; + // bias cancelation +// roll_angle = roll_angle - 2.2f/180.0f*pi; + // roll angle of turning command //////////////////////////////////////////////////////////////////////////////////////////// + roll_ref = lpf(roll_cmd, roll_old, 3.1415f); + roll_old = roll_ref; + // steering angle of turning command //////////////////////////////////////////////////////////////////////////////////////// + /* + -L*g + steer_ss = --------------- * roll_ss + V*V*cos(lammda) + */ + steer_ref = -L*g/velocity/velocity/cos(lammda)*roll_ref; + // controller //////////////////////////////////////////////////////////////////////////////////////////////////////////////// + if(start_control == 0) { + deg_s = (180.0f + steer_out)*4096.0f/360.0f; + } + if(start_control == 1) { + + // lpf for velocity, cut-off freq. = 3Hz = 18.85 (rad/s) + velocity = lpf(velocity, velocity_old, 18.85); + velocity_old = velocity; + + controller(velocity); + steer_angle(u, velocity); + + //Steer output + deg_s = (180.0f - steering_angle)*4096.0f/360.0f; + // count to turn stright + if(count2straight < CNT2STR) count2straight++; + if(count2straight == CNT2STR) roll_cmd = 2.0f/180.0f*pi; + } + // send data by UART ////////////////////////////////////////////////////////////////////////////////////////////////////////// + uart_send(); +}// timer1_interrupt + +// timer1_interrupt ********************************************************************************************************************************************* +void timer2_interrupt(void) +{ + dynamixelClass.servo(Steering_SERVO_ID,deg_s,1000); +}// timer2_interrupt + +// read_sensor ************************************************************************************************************************************************** +void read_sensor(void) +{ + // sensor raw data + Gyro_axis_data[0] = sensor.readRawGyroX(); + Gyro_axis_data[1] = sensor.readRawGyroY(); + Gyro_axis_data[2] = sensor.readRawGyroZ(); + + Acce_axis_data[0] = sensor.readRawAccelX(); + Acce_axis_data[1] = sensor.readRawAccelY(); + Acce_axis_data[2] = sensor.readRawAccelZ(); +}// read_sensor + +// uart_send **************************************************************************************************************************************************** +void uart_send(void) +{ + // uart send data + display[0] = roll_angle/pi*180.0f*100.0f; + display[1] = droll_angle/pi*180.0f*100.0f; + display[2] = speed*100; + display[3] = steering_angle*100; + display[4] = u_1*100; + display[5] = u_2*100; + + separate(); + + int j = 0; + int k = 1; + while(k) { + if(BT_Data.writeable()) { + BT_Data.putc(num[i]); + i++; + j++; + } + if(j>1) {// send 2 bytes + k = 0; + j = 0; + } + } + if(i>13) i = 0; +}// uart_send + +//separate **************************************************************************************************************************************************** +void separate(void) +{ + num[2] = display[0] >> 8; // HSB(8bit)資料存入陣列 + num[3] = display[0] & 0x00ff; // LSB(8bit)資料存入陣列 + num[4] = display[1] >> 8; + num[5] = display[1] & 0x00ff; + num[6] = display[2] >> 8; + num[7] = display[2] & 0x00ff; + num[8] = display[3] >> 8; + num[9] = display[3] & 0x00ff; + num[10] = display[4] >> 8; + num[11] = display[4] & 0x00ff; + num[12] = display[5] >> 8; + num[13] = display[5] & 0x00ff; +}//separate \ No newline at end of file
diff -r 000000000000 -r bf9bf4b7625f mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Jun 08 14:11:49 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/e7ca05fa8600 \ No newline at end of file