Week 16 - ngetachew/Portfolio GitHub Wiki

On Monday, I wasn't able to do much with the Pi because one of my partners was working on the Delta Arm, so I couldn't run any code concurrently with theirs. Instead, I helped another on of my partners with the graphical interface of the game. We planned to have a Surface Pro accept user input, but Kivy wasn't allowing us to use the touch screen. In the few moments I had with the Pi, I was testing out the code to use the indeffector's motor. The problem we had was that the gears would grind together and stall on certain parts of the motor. I tested this by running code that I had been told was working last year, and Harlow's Power Plank. However, none of these fixed the problem, so we asked our mentors for new gears, which should arrive soon.

On Tuesday, me and one of my partners made a little bit of progress on the touchscreen, but I was more focused on the Bear's path finding algorithm. At this point, Shaeer came and told our group that our number one priority should be getting the Delta Arm to be fully functional. This lead me to stop working on the Bear and start working on getting the individual functions to work. For the next three days, all I did was debug and troubleshoot the move_to_point() and 'move_to_point_in_straight_line()function. Themove_to_point()` function was intended to move the Delta Arm using a Cartesian coordinate plane. Of course, it wasn't working initially, the arm had variables that were reliant on measurements that needed to be remeasured. We were able to use inclinometers and the CAD file:

After this I began to work on 'move_to_point_in_straight_line(). This proved to be an enormous task. Interestingly, the function that we were provided with actually moved the arm in an arc rather than a straight line. This function uses various other Delta Arm functions, and I found that these were the source of the error. Specifically, the forward_kinematics()_functions, which is supposed to take three Cartesian coordinates and turn them into angles for the motor to adjust itself to. To fix this, I found an alternative function online that accomplishes the same thing. The function was in C, but I converted it to python. Here's the original:int delta_calcForward(float theta1, float theta2, float theta3, float &x0, float &y0, float &z0) { float t = (f-e)*tan30/2; float dtr = pi/(float)180.0;

 theta1 *= dtr;
 theta2 *= dtr;
 theta3 *= dtr;

 float y1 = -(t + rf*cos(theta1));
 float z1 = -rf*sin(theta1);

 float y2 = (t + rf*cos(theta2))*sin30;
 float x2 = y2*tan60;
 float z2 = -rf*sin(theta2);

 float y3 = (t + rf*cos(theta3))*sin30;
 float x3 = -y3*tan60;
 float z3 = -rf*sin(theta3);

 float dnm = (y2-y1)*x3-(y3-y1)*x2;

 float w1 = y1*y1 + z1*z1;
 float w2 = x2*x2 + y2*y2 + z2*z2;
 float w3 = x3*x3 + y3*y3 + z3*z3;
 
 // x = (a1*z + b1)/dnm
 float a1 = (z2-z1)*(y3-y1)-(z3-z1)*(y2-y1);
 float b1 = -((w2-w1)*(y3-y1)-(w3-w1)*(y2-y1))/2.0;

 // y = (a2*z + b2)/dnm;
 float a2 = -(z2-z1)*x3+(z3-z1)*x2;
 float b2 = ((w2-w1)*x3 - (w3-w1)*x2)/2.0;

 // a*z^2 + b*z + c = 0
 float a = a1*a1 + a2*a2 + dnm*dnm;
 float b = 2*(a1*b1 + a2*(b2-y1*dnm) - z1*dnm*dnm);
 float c = (b2-y1*dnm)*(b2-y1*dnm) + b1*b1 + dnm*dnm*(z1*z1 - re*re);

 // discriminant
 float d = b*b - (float)4.0*a*c;
 if (d < 0) return -1; // non-existing point

 z0 = -(float)0.5*(b+sqrt(d))/a;
 x0 = (a1*z0 + b1)/dnm;
 y0 = (a2*z0 + b2)/dnm;
 return 0;

}`