Dammenisvoorcasuals / Mbed 2 deprecated chessRobot

control for robotic arm that can play chess using a granular gripper

Dependencies:   Encoder mbed HIDScope Servo MODSERIAL

Fork of chessRobot by a steenbeek

Files at this revision

API Documentation at this revision

Revision 121:6d8f1bdcda05, committed 2015-10-28

Comitter:
annesteenbeek
Date:
Wed Oct 28 13:38:36 2015 +0100
Parent:
120:440f1516101b
Child:
122:1a5df0765790
Child:
124:f67ce69557db
Commit message:
set debugging for kinematics

Changed in this revision

actuators.cpp Show annotated file Show diff for this revision Revisions of this file
debug.cpp Show annotated file Show diff for this revision Revisions of this file
debug.h Show annotated file Show diff for this revision Revisions of this file 
--- a/actuators.cpp	Wed Oct 28 11:22:48 2015 +0000
+++ b/actuators.cpp	Wed Oct 28 13:38:36 2015 +0100
@@ -268,22 +268,24 @@
 const double Xmin = -0.3;
 const double Ymax = 0.645;
 const double Ymin = 0.33;
+double Xpos = 0; // set values
+double Ypos =0;
 
 
 
 bool kinematics(){
     // calculate current x and Y
-    double X = (L2+L3)*cos((motor1Pos + motor2Pos)*PI/180) + L1*cos(motor1Pos*PI/180);
-    double Y = (L2+L3)*sin((motor1Pos + motor2Pos)*PI/180) + L1*sin(motor1Pos*PI/180);
+    Xpos = (L2+L3)*cos((motor1Pos + motor2Pos)*PI/180) + L1*cos(motor1Pos*PI/180);
+    Ypos = (L2+L3)*sin((motor1Pos + motor2Pos)*PI/180) + L1*sin(motor1Pos*PI/180);
     // check if x and y are within limits
     //  else  Store the constraint line
     //      check if movement is in direction of constraint
     //      else return false no movement (anglespeed = 0)
     // calculate required angle speeds
-    if( (X>Xmax && setXSpeed > 0 )|| \
-        (X<Xmin && setXSpeed < 0 )|| \
-        (Y>Ymax && setYSpeed > 0 )|| \
-        (Y<Ymin && setYSpeed < 0 )   \
+    if( (Xpos>Xmax && setXSpeed > 0 )|| \
+        (Xpos<Xmin && setXSpeed < 0 )|| \
+        (Ypos>Ymax && setYSpeed > 0 )|| \
+        (Ypos<Ymin && setYSpeed < 0 )   \
     ){
         motor1SetSpeed = 0;
         motor2SetSpeed = 0;

--- a/debug.cpp	Wed Oct 28 11:22:48 2015 +0000
+++ b/debug.cpp	Wed Oct 28 13:38:36 2015 +0100
@@ -28,11 +28,12 @@
 HIDScope scope(5);
 
 void debugProcess(){
-    scope.set(0, motor2Pos);
+    scope.set(0, motor1SetSpeed);
 	scope.set(1, motor2SetSpeed);
-	scope.set(2, motor2Speed);
-	scope.set(3, motor2PWM);
-	scope.set(4, 1300 + 700*servoSpeed);
+	scope.set(2, setXSpeed);
+	scope.set(3, motor1PWM);
+	scope.set(4, motor2PWM);
+    scope.set(5, Xpos)
     scope.send();
 }
 

--- a/debug.h	Wed Oct 28 11:22:48 2015 +0000
+++ b/debug.h	Wed Oct 28 13:38:36 2015 +0100
@@ -14,6 +14,9 @@
 extern double prevTime;
 extern double servoSpeed;
 
+extern double Xpos;
+extern double Ypos;
+
 extern PID motor2PID;
 extern DigitalIn button2;
 extern AnalogIn pot2;