Chris LU
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
--- /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
--- /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
--- /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
--- /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
+ }
+}
+
+
--- /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
--- /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
--- /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
--- /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
--- /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
--- /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