Control problem

Lynxmotion Electronics & Software
1

Hi,

I am working on a 7DOF manipulator which will trace a given trajectory in xyz space, here is some information about the system i am using,

PC OS: windows 7
programing language&compiler: Visual studio C++
SSC32 firmware version: 2.07

The manipulator i am working on is actually a Cyton Robai, since they are using SSC32 control board so i am trying to direct control the manipulator by sending the command to SSC32 through serial port.

So here is the problem,

In the video i am trying to control the manipulator tracing a circle trajectory, however there are a lot of “vibration” is generated and apparently it is not accurate at all, so my question is if there is anyway to solve this kind of vibration problem from “software level”?

In my case, i am generate a set of joint angle from matlab and than passing it to the C++ and storing in a vector. And then i just transfer those joint angle into the command and send them to the ssc32, time step i’m using to update the coordinate is 100ms.

Here is a part of my code about sending the command to the ssc32,

for (int i = 0; i<NumJoint; i++)
			{
				joint_angle* = AngleCalibrate(q*);
				std::cout<<"joint_angle "<<i<<"="<<joint_angle*<<"\n";
			}

//writing joint angle to controller
char Angle[200];
DWORD dwBytesToWrite = (DWORD)strlen(Angle);
DWORD dwBytesWritten = sizeof(Angle) + 1;

std::string String_char;
			
String_char = sprintf(Angle,"#0 P%d T%d #1 P%d T%d #2 P%d T%d #3 P%d T%d #4 P%d T%d #5 P%d T%d #6 P%d T%d \n\r",joint_angle[0],TimeStep,joint_angle[1],TimeStep,joint_angle[2],TimeStep,joint_angle[3],TimeStep, joint_angle[4],TimeStep,joint_angle[5],TimeStep,joint_angle[6],TimeStep);

			printf(Angle);
			std::cout<<"\n";

			if(!WriteFile(hComm, Angle,dwBytesToWrite, &dwBytesWritten, NULL))
			{
				printf("error sending command");
				ERROR_DETAIL();
				system("pause");
			}
			Sleep(100);

thanks you***

2

Not a C programmer but it looks to me like you are sending; servo, position, time, servo, position, time, etc. This is not correct. Do it like this; servo, position, servo, position, etc, time. The time applies to all servos, time can not apply to each servo independently. Also strive for a 30mS time frame, not 100.

3

Did I miss something? This looks like code to write a single position of the arm (all seven joints), I don’t see a loop that is moving through an array of positions for the arm.

Break up the moves into many smaller moves between arm positions.

Alan KM6VV

4

It looks also like he’s just using one power supply for the whole thing. From what I can tell, the commands are supposed to be sent consistently, but the LED on the SSC-32 is sporadic. Try using a separate power source for the SSC-32’s logic.

Alan…

The positions are set elsewhere.

5

Yeah, OK, just part. We’d probably need to see the “other part”, the loop where the data from the table are dispatched. If the moves are far enough, then breaking them up as I suggested (like in the hexapod code) would help. With finer control, one would use acceleration/deceleration between way-points (positions along the tool path). But we can’t see any of that (or lack thereof) in the code snippet supplied.

Alan KM6VV

6

ok, here is the code for my motion loop,

		mxArray *convertion, *T = NULL, *result = NULL; //define matlab array
		double *init;		// joint angle initial condition (double array)
		convertion = mxCreateDoubleMatrix(1,7,mxREAL);//define size of convertion
		T = mxCreateDoubleMatrix(1,2,mxREAL);
		double time_counter = 0;
		double stop_time = 30;//stop time
		double t] = {0,0}; //time vector in C++ space
		double *q; //raw joint angle from matlab
		int joint_angle[7];//joint angle after calibrate and ready to control
		int TimeStep = 30; // time step in ms
		init = mxGetPr(convertion);
		for (int i = 0; i <NumJoint;i++)
		{
			init* = 0.442;
		}
		memcpy((void*)mxGetPr(convertion),(void*)init,sizeof(init));//transfer data from doubel array init to matlab array convertion

		//pass variable init to matlab
		engPutVariable(engine,"q_init",convertion);


		while (time_counter <= stop_time)//initial of motion loop
		{
			//passing time information to matlab
			t[0] = t[1];
			memcpy((void*)mxGetPr(T),(void*)t,sizeof(t));
			engPutVariable(engine,"tm",T);
			std::cout<<"current time is"<<t[0]<<"\n";

			//initial matlab command to calculate next joint angle
			engEvalString(engine, "tf = tm(1)");
			engEvalString(engine,"q_t = LAB_joint_angle_test(tf,q_init);");

			//pass joint angle information from matlab to c++
			if((result = engGetVariable(engine,"q_t")) == NULL)
			{
				printf("error passing information from matlab to c++");
			}
			else
			{
				q = (double*)mxGetPr(result);
			}
		
			//translate joint angle
			for (int i = 0; i<NumJoint; i++)
			{
				joint_angle* = AngleCalibrate(q*);
				std::cout<<"joint_angle "<<i<<"="<<joint_angle*<<"\n";
			}

			//writing joint angle to controller
			char Angle[200];
			DWORD dwBytesToWrite = (DWORD)strlen(Angle);
			DWORD dwBytesWritten = sizeof(Angle) + 1;

			std::string String_char;
			
			String_char = sprintf(Angle,"#0 P%d #1 P%d #2 P%d #3 P%d #4 P%d #5 P%d #6 P%d T%d \n\r",
									    joint_angle[0],joint_angle[1],joint_angle[2],joint_angle[3],
									    joint_angle[4],joint_angle[5],joint_angle[6],TimeStep);
			printf(Angle);
			std::cout<<"\n";

			if(!WriteFile(hComm, Angle,dwBytesToWrite, &dwBytesWritten, NULL))
			{
				printf("error sending command");
				ERROR_DETAIL();
				system("pause");
			}
			Sleep(25);
			time_counter = time_counter + 0.03;
			t[1] = t[1]+0.03;	
		}

Alan,
you said we could control the acceleration and declaration? can you tell me how to do that in code?****

7

hi,

i’m not sure i catch you, how is the power supply to the logic could affect the motion?
oh, actually that led is not the led of SSC32, it is the led on the serial port cable.

8

Sorry, I’m not yet seeing a list of waypoints, or a function getting/passing the “next point” to your code.

For accl/decel, you take the distance between a pair of points, and split it up. Issue only a portion of the move at a time. Give the steps a ramping up (accl) time to complete, say for the first several steps. Then a longer move (but not all the way), and then the following steps are issued also ramping, but slowing down (decel). the key is to splitting up the move between each commanded point. Usually you define the two ramps in terms of either speed or distance, and use a table. You might work in degrees, or R/C uS. the move profiles then look either like a triangle, or a truncated triangle. If you barely have time to accelerate before you have to decelerate, then you get a triangular profile. If you have a long move, then your accelerate first, then have a “flat” period of slew, and then you start your deceleration. Just taking a couple of “short steps” at partial speed at each end of a move might be all you need.

Alan KM6VV

9

Do ya think ya might be able to post an image of your wiring so we could maybe eliminate it as a possibility of causing your problem?.. It’d take only a second and could eliminate a lot of pointless banter concerning code…

10

I’m talking about the green LED on the SSC-32. It is clearly visible in the last half of the video.
If you are using the same power source for the SSC-32’s logic AND the servos, then you could be having a power issue here. If the servos draw too much current, then the voltage will drop, causing the SSC-32 to reset. This will cause the servos to drop limp. Since you are sending commands every 100ms, it would look jittery like in your video. So, if you have not done so already, try connecting your normal power source to VS1 and a 9v battery to VL. Be sure to remove the VS1=VL jumper!

11

its finally work, although the trajectory is still not too accurate.

The major issue is the speed of i’m updating the coordinate, as you guys suggested, using smaller time step and multiple step to arrive destination. Some other point is previously i’m using ODE solver from matlab, transferring the data between C++ and matlab cause additional time, while i’m using the ODE solver in C++, the problem is basically solved.

Thanks for your help again, thanks