Fixes #4 issue on github: initial kick/jerk

Fixes the initial kick/jerk issue during motor start up.

include/motor_driver/MotorDriver.hpp

@@ -185,6 +185,9 @@ namespace motor_driver
         };
 
         // Default Motor Messages
+
+        motorCommand zeroCmdStruct = {0, 0, 0, 0, 0};
+
         unsigned char motorEnableMsg[8] = {0xFF,
                                            0xFF,
                                            0xFF,

src/MotorDriver.cpp

@@ -100,6 +100,25 @@ namespace motor_driver
             //     std::cout << "MotorDriver::disableMotor() Motor seems to already be in disabled state. \
             //                   Did you want to really do this?" << std::endl;
             // }
+
+            // Bugfix: To remove the initial kick at motor start.
+            // The current working theory is that the motor "remembers" the last command. And this
+            // causes an initial kick as the motor controller starts. The fix is then to set the 
+            // last command to zero so that this does not happen. For the user, the behaviour does
+            // not change as zero command + disable is same as disable.
+            bool return_val = encodeCANFrame(zeroCmdStruct, CANMsg_);
+            MotorCANInterface_.sendCANFrame(disable_motor_ids[iterId], CANMsg_);
+            usleep(motorReplyWaitTime);
+            if (MotorCANInterface_.receiveCANFrame(CANReplyMsg_))
+            {
+                state = decodeCANFrame(CANReplyMsg_);
+            }
+            else
+            {
+                perror("MotorDriver::disableMotor() Unable to Receive CAN Reply.");
+            }
+
+            // Do the actual disabling after zero command.
             MotorCANInterface_.sendCANFrame(disable_motor_ids[iterId], motorDisableMsg);
             usleep(motorReplyWaitTime);
             if (MotorCANInterface_.receiveCANFrame(CANReplyMsg_))
@@ -147,7 +166,7 @@ namespace motor_driver
                 perror("MotorDriver::setZeroPosition() Unable to Receive CAN Reply.");
             }
 
-            while (state.position > (5 * (pi / 180)))
+            while (state.position > (1 * (pi / 180)))
             {
                 MotorCANInterface_.sendCANFrame(zero_motor_ids[iterId], motorSetZeroPositionMsg);
                 usleep(motorReplyWaitTime);

src/MotorDriverTest.cpp

@@ -22,27 +22,27 @@ uint64_t get_time_in_microseconds()
 
 int main(int argc, char **argv)
 {
-    // vector<int> motor_ids = {0x06, 0x07};
-	vector<int> motor_ids = {0x02, 0x03};
+	vector<int> motor_ids = {0x08, 0x09};
 	motor_driver::MotorDriver motor_controller(motor_ids, "can0", motor_driver::MotorType::AK80_6_V1p1);
 
+	cout<<"Setting Zero Position..."<<endl;
+    auto stateZero = motor_controller.setZeroPosition(motor_ids);
+    cout<<"1. Zero Position: "<<stateZero[motor_ids[0]].position<<" Velocity: "<<stateZero[motor_ids[0]].velocity<<" Torque: "<<stateZero[motor_ids[0]].torque<<endl; 
+	cout<<"2. Zero Position: "<<stateZero[motor_ids[1]].position<<" Velocity: "<<stateZero[motor_ids[1]].velocity<<" Torque: "<<stateZero[motor_ids[1]].torque<<endl; 
+
+	std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+
 	cout<<"Enabling Motor..."<<endl;
 	auto start_state = motor_controller.enableMotor(motor_ids);
 	cout<<"1. Enable Position: "<<start_state[motor_ids[0]].position<<" Velocity: "<<start_state[motor_ids[0]].velocity<<" Torque: "<<start_state[motor_ids[0]].torque<<endl;
 	cout<<"2. Enable Position: "<<start_state[motor_ids[1]].position<<" Velocity: "<<start_state[motor_ids[1]].velocity<<" Torque: "<<start_state[motor_ids[1]].torque<<endl;
-
-    cout<<"Setting Zero Position..."<<endl;
-    auto stateZero = motor_controller.setZeroPosition(motor_ids);
-    cout<<"1. Zero Position: "<<stateZero[motor_ids[0]].position<<" Velocity: "<<stateZero[motor_ids[0]].velocity<<" Torque: "<<stateZero[motor_ids[0]].torque<<endl; 
-	cout<<"2. Zero Position: "<<stateZero[motor_ids[1]].position<<" Velocity: "<<stateZero[motor_ids[1]].velocity<<" Torque: "<<stateZero[motor_ids[1]].torque<<endl; 
 
 	int i;
 	cout << "Waiting for user input... Enter any number to continue..." << endl;
 	cin >> i;
 
 	motor_driver::motorCommand commandStruct1 = {1.57, 0, 50, 2, 0}; 
 	motor_driver::motorCommand commandStruct2 = {-1.57, 0, 50, 2, 0}; 
-	// std::map<int, motor_driver::motorCommand> commandMap = {{6, commandStruct}, {7, commandStruct}};
 	std::map<int, motor_driver::motorCommand> commandMap = {{motor_ids[0], commandStruct1}, {motor_ids[1], commandStruct2}};
 	auto startT = get_time_in_microseconds();
 	auto commandState = motor_controller.sendRadCommand(commandMap);