Diff: main.cpp

Revision:

2:da3b7dd2beb0

Parent:

1:4465c9e203ce

Child:

3:337345f748cf

--- a/main.cpp	Thu Oct 15 10:42:30 2015 +0000
+++ b/main.cpp	Sun Oct 18 13:19:55 2015 +0000
@@ -3,6 +3,10 @@
 #include "MODSERIAL.h"
 #include "QEI.h"
 
+//Aray of locations
+float X [10] = {-4, -3, -2, -1, 0, 1, 2, 3, 4};
+float Y [10] = {-4, -3, -2, -1, 0, 1, 2, 3, 4};
+
 //---------- Change motor control parameters-------
 //Motor 1
 const float M1_Ks = 0.4;           //minimum power required to move
@@ -38,10 +42,12 @@
 double M1_dpos, M1_pos;
 double M2_dpos, M2_pos;
 double M_error, M1_error, M2_error;
+double dR, A_R, L_error, M1_dTheta;
 double position;
 double pref,pref2;
 bool M1_dir, M2_dir;
-const double long gearbox=0.08587786259541984732824427480916;
+const double long gearbox = 0.08587786259541984732824427480916;
+const double long PI = 3.14159265359;
 double M_error1 = 0;
 
 //Timers and Tickers
@@ -89,17 +95,46 @@
 //Desired Position Motors --> future script emg
 void Setpoint()
 {
-    M1_dpos=pot1.read()*360-180;
-    M2_dpos=pot2.read()*360-180;
-    pc.printf(" Desired: M1_dpos: %f M1_dir: %i M2_dpos: %f M2_dir: %i \n",M1_dpos, M1_dir, M2_dpos, M2_dir);
+    //New version of setpoint which takes board into account
+    //set desired x and y point in array
+    int dX = pot1.read() * 10;
+    int dY = pot2.read() * 10;
+
+    //Set the length of the arm
+    dR = sqrt( X[dX] * X[dX] + Y[dY] * Y[dY]);//desired length of the arm
+    M1_dTheta = acos( Y[dY] / X[dX] );          //desired angle of the arm (theta for M1) //Aandacht is dit graden?!!!!!
 }
+
+void Length arm()
+{
+
+    //read out length of arm
+    double A_Rx = L1 * cos( M1 * PI / 180.0) + L2 * cos( (M1 + M2)* PI / 180.0);
+    double A_Ry = L1 * sin( M1 * PI / 180.0) + L2 * sin( (M1 + M2)* PI / 180.0);
+    A_R = sqrt( pow (A_Rx, 2) + pow (A_Ry, 2));
+
+    L_error = dR-A_R;
+    double ThetaP = 0;
+    double dTheta = ThetaR+ThetaP;//rekening houden met theta 2
+    double M2_dTheta = 0;
+
+}
+
+void setlength(){
+    L_error = dR-A_R;
+    int Direction = (A_R > dR) ? false:true;
+    }
+
 double Pw_control (double Pulse,double setpoint,double Ks, double Kp,double Ki,double Kd, double friction)
 {
     //read position motor
     position = gearbox*Pulse;
     // Motor Power
     M_error = abs(setpoint-position);
-    double M_error_Int= M_error_Int+M_error/1e4;
+    
+    
+    
+    double M_error_Int = M_error_Int+M_error/1e4;
     double Ps = Ks;
     double Pp = Kp * M_error;                                        //Proportional control
     double Pi = Ki * M_error_Int*M_error;  //check dit --> nog niet goed                            //int controll
@@ -131,11 +166,11 @@
 // begin to set point
 void calibration()
 {
-    M1_dpos=0;
-    M2_dpos=0;
-    if (button==true) {
-        M1_pos=0;
-        M2_pos=0;
+    M1_dpos = 0;
+    M2_dpos = 0;
+    if (button == true) {
+        M1_pos = 0;
+        M2_pos = 0;
     } else {
     }
 }
@@ -145,7 +180,7 @@
 int main()
 {
     //turn that led off!It hurts my eyes! Ow, I do boot.
-    myled=1;
+    myled = 1;
 
     //PWM period motors
     E1.period(0.0001f);
@@ -180,7 +215,7 @@
             //print error
             pc.printf("M1_pos = %f M1_error = %f M2_pos = %f M2_error = %f\n", M1_pos, M1_error, M2_pos, M2_error); // once this works place it in 'send'
             control_go = false;
-        } else if (send_go==true) {
+        } else if (send_go == true) {
             Send();
 
             send_go = false;