Revision 2:703501924009, committed 2019-10-18

Comitter:

s1961438

Date:

Fri Oct 18 12:07:54 2019 +0000

Parent:

1:b862262a9d14

Commit message:

Test_InverseKinematics

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RKI/InverseKinematics.cpp	Fri Oct 18 12:07:54 2019 +0000
@@ -0,0 +1,53 @@
+#include "...."
+#include "math.h"
+
+int Theta1(float Angle)
+{
+    int Compare;
+    int min_comp=1450;
+    int max_comp=6500;
+    int min_angle=0;
+    int max_angle= 180;
+    Compare=((max_comp-min_comp)/(max_angle-min_angle))*(Angle-min_angle)+
+    return Compare;
+}
+
+int Theta2(float Angle)
+{
+    int Compare;
+    int min_comp=1500;
+    int max_comp=6900;
+    int min_angle=-90;
+    int max_angle= 90;
+    Compare=((max_comp-min_comp)/(max_angle-min_angle))*(Angle-min_angle)+
+    return Compare;  
+}
+
+int main(void) 
+{ 
+    float X=1.0; //desired X position of the end effector in cm
+    float Y=1.0; //desired Y position of the end effector in cm 
+    float r1=0.0;
+    float phi1=0.0;
+    float phi2=0.0;
+    float phi3=0.0;
+    float a2=6.0;
+    float a4=5.5:
+    float T1=0.0; //T1 is theta 1 in radians
+    float T2=0.0; //T2 is theta 2 in radians
+    
+    r1=sqrt(X*X+Y*Y); //eguation 1
+    phi1=acos(((a4*a4)-(a2*a2)-(r1*r1))/(-2.0*a2*r1)); //Equation2
+    phi2=atan(Y/X); //Eguation 3
+    T1=phi2-phi1;
+    phi3=acos(((r1*r1)-(a2*a2)-(a4*a4))/(-2.0*a2*a4)); //Equation 5
+    T2=3.14159-phi3; //Equation 6
+    
+    PWM_1_Start();
+    for(;;)
+    {
+        PWM_1_WriteCompare1(Theta1((T1/3.14159)*180.0)); 
+        PWM_1_WriteCompare2(Theta2((T2/3.14159)*180.0));
+        CyDelay(2000);
+    
+    
\ No newline at end of file

--- a/main.cpp	Wed Sep 04 15:30:13 2019 +0000
+++ b/main.cpp	Fri Oct 18 12:07:54 2019 +0000
@@ -1,23 +1,24 @@
 #include "mbed.h"
-//#include "HIDScope.h"
-//#include "QEI.h"
-#include "MODSERIAL.h"
-//#include "BiQuad.h"
-//#include "FastPWM.h"
-
-DigitalOut led(LED_RED);
-
-MODSERIAL pc(USBTX, USBRX);
+  #include "HIDScope.h"        // Require the HIDScope library
+ 
+  HIDScope    scope(2);        // Instantize a 2-channel HIDScope object
+  Ticker      scopeTimer;      // Instantize the timer for sending data to the PC 
+  
+  AnalogIn    a0(A0);          // Using an analog input to obtain data 
+  
+  int main()
+  {
+    
+    // Attach the HIDScope::send function to the timer at a 10.000 us interval (100 Hz)
+    scopeTimer.attach_us(&scope, &HIDScope::send, 1e4);
+    
+    // Read from the analog input in an endless loop. Two channels are written each iteration. 
+    // Note that the control loop can run at a different frequency (1 kHz in this case)
+    while(1){
+        scope.set(0, a0.read());
+        scope.set(1, a0.read());
+        wait_us(1000);
+    };    
+    
+  }   
 
-int main()
-{
-    pc.baud(115200);
-    pc.printf("\r\nStarting...\r\n\r\n");
-    
-    while (true) {
-        
-        led = !led;
-        
-        wait_ms(500);
-    }
-}

--- a/mbed.bld	Wed Sep 04 15:30:13 2019 +0000
+++ b/mbed.bld	Fri Oct 18 12:07:54 2019 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/mbed_official/code/mbed/builds/d1b4690b3f8b
\ No newline at end of file
+https://os.mbed.com/users/mbed_official/code/mbed/builds/675da3299148
\ No newline at end of file