Revision 4:ca797e7daaf4, committed 2017-11-07

Comitter:

Frimzenner

Date:

Tue Nov 07 10:45:06 2017 +0000

Parent:

3:c63c0a23ea21

Commit message:

x and y pure emg movement, removed modserial statements (No reach limiter active, see MotorAngle())

Changed in this revision

--- a/main.cpp	Fri Nov 03 11:09:30 2017 +0000
+++ b/main.cpp	Tue Nov 07 10:45:06 2017 +0000
@@ -9,7 +9,6 @@
 #include "iostream"
 #include "BiQuad.h"
 #include "TextLCD.h"
-#include "MODSERIAL.h"
 
 //intialize all pins
 PwmOut motor1(D5);
@@ -29,7 +28,6 @@
 DigitalOut bit1(D2);
 DigitalOut bit2(D14);
 DigitalOut bit3(D15);
-MODSERIAL pc(USBTX, USBRX);
 
 //initialize variables
 const float pi = 3.14159265358979323846; //value for pi
@@ -331,8 +329,8 @@
 //Works for all 4 quadrants of the (unit) circle
     xe = x_des;
     ye = y_des;
-    
-   //while(pow(xe, 2)+pow(ye,2) > pow(l1+l2, 2)){ //when attempted to go to a point out of reach
+
+    //while(pow(xe, 2)+pow(ye,2) > pow(l1+l2, 2)){ //when attempted to go to a point out of reach
 //       if (y_des == 0){    //make sure you do not divide by 0 if ye == 0
 //           xe = x_des - 1;
 //       } else {
@@ -348,7 +346,7 @@
 //   }
 
 
-   // while(pow(xe, 2)+pow(ye, 2) > pow(l1+l2, 2)) {
+    // while(pow(xe, 2)+pow(ye, 2) > pow(l1+l2, 2)) {
 //        if (xe > 0) {            //right hand plane
 //            if (ye > 0) {        //positive x & y            QUADRANT 1
 //                xe = x_des - (x_des/y_des);   //go to a smaller xe point on the same line
@@ -401,9 +399,6 @@
     rad2rot = radDeg/360;
     desiredAngle1 = q1 * rad2rot;
     desiredAngle2 = q2 * rad2rot;
-    pc.printf("\n New values \n\r");
-    pc.printf("xloc = %f, yloc = %f \n \r", xe, ye);
-    pc.printf("q1 = %f[rad], desA1 = %f [abs rot], q2 = %f[rad], desA2 = %f[abs rot]\n\r", q1, desiredAngle1, q2, desiredAngle2);
 }
 
 void motorButtonController()
@@ -439,11 +434,6 @@
     wait(2);
     xe = x_des;
     ye = y_des;
-    pc.baud(115200);
-    pc.printf("\n Start up complete \n \r");
-    pc.printf("initial values: \n \r");
-    pc.printf("xloc = %f, yloc = %f \n \r", xe, ye);
-    pc.printf("q1 = %f[rad], desA1 = %f [abs rot], q2 = %f[rad], desA2 = %f[abs rot]\n \r", q1, desiredAngle1, q2, desiredAngle2);
     myControllerTicker.attach(&motorButtonController, controllerTickerTime);
     bit1=0;
     bit2=0;
@@ -462,38 +452,28 @@
     stateChange=true;
 
     while(true) {
-        if(!button1) {
-            x_des -= positionIncrement;
+        if(palmarisAvg*palmarisMax > 0.9) {
+            y_des -= positionIncrement;
             motorAngle();
-            wait(0.4f);
+            wait(0.3f);
         }
 
-        if(!button2) {
-            x_des += positionIncrement;
+        if(carpiFlexAvg*carpiFlexMax > 0.9) {
+            y_des += positionIncrement;
             motorAngle();
-            wait(0.4f);
+            wait(0.3f);
         }
-        if(bicepsAvg*bicepsMulti > 0.9){
+        if(bicepsAvg*bicepsMulti > 0.9) {
             x_des -= positionIncrement;
             motorAngle();
-            wait(0.5f);    
+            wait(0.3f);
         }
-        if(tricepsAvg*tricepsMulti > 0.9){
+        if(tricepsAvg*tricepsMulti > 0.9) {
             x_des += positionIncrement;
             motorAngle();
-            wait(0.5f);
+            wait(0.3f);
         }
-//
-//        if(button3) {
-//            x_des += positionIncrement;
-//            motorAngle();
-//            wait(0.3f);
-//        }
-//        if(button4) {
-//            x_des -= positionIncrement;
-//            motorAngle();
-//            wait(0.3f);
-//        }
+
         if(stateChange) {
             switch(state) {
                 case motorCalib  :
@@ -579,17 +559,17 @@
                 case EMGControl  :
                     stateChange=false;
                     sendState(EMGControl);
-                    if(bicepsMax != 0){
-                    bicepsMulti=1/bicepsMax;
+                    if(bicepsMax != 0) {
+                        bicepsMulti=1/bicepsMax;
                     }
-                    if(tricepsMax != 0){
-                    tricepsMulti=1/tricepsMax;
+                    if(tricepsMax != 0) {
+                        tricepsMulti=1/tricepsMax;
                     }
-                    if(carpiFlexMax != 0){
-                    carpiFlexMulti=1/carpiFlexMax;
+                    if(carpiFlexMax != 0) {
+                        carpiFlexMulti=1/carpiFlexMax;
                     }
-                    if(palmarisMax != 0){
-                    palmarisMulti=1/palmarisMax;
+                    if(palmarisMax != 0) {
+                        palmarisMulti=1/palmarisMax;
                     }
                     break;
             }