Diff: main.cpp

Revision:

12:e591729e854a

Parent:

11:4382c567e0d4

Child:

13:4f426186cd19

--- a/main.cpp	Fri Oct 28 07:35:36 2016 +0000
+++ b/main.cpp	Fri Oct 28 09:45:31 2016 +0000
@@ -5,6 +5,8 @@
 #include "QEI.h"
 #include "BiQuad.h"
 #include "motor.h"
+#include "EMG_input.h"
+#include "HIDScope.h"
 // Angle limits 215, 345 lower arm, 0, 145 upper arm
 // Robot arm x,y limits (limit to table top)
 
@@ -45,6 +47,8 @@
 bool flag_output;
 bool flag_EMG;
 
+    HIDScope scope(5);
+
 Ticker mainticker;  // Main ticker
 
 /*
@@ -70,6 +74,12 @@
 bool moveleft;  // Flag set to true if setpoint moving left
 bool movedown;  // Flag set to true if setpoint moving down
 
+/*
+** EMG variables
+*/
+
+EMG_input emg1(A0);
+EMG_input emg2(A1);
 
 // Struct r_link:
 // Defines a robot link (arm or end effector).
@@ -251,7 +261,7 @@
 */
 void robot_init() {
     // Set pwm motor periods
-    gripperpwm.period_ms(20);
+ //   gripperpwm.period_ms(20);
     
     pc.baud(115200);    // Set serial communication speed
 
@@ -286,14 +296,14 @@
 */
 void r_moveHorizontal(){
     if (flag_switch){
-        if (!switch1.read()){
+        if (!switch1.read() || emg1.read()){
             moveleft = true;
             vx = (vx<-maxspeed)?-maxspeed:(vx-ax);
         }
         else {
             vx = moveleft?0:vx;
         }
-        if (!switch2.read()){
+        if (!switch2.read() || emg2.read()){
             moveleft = false;
             vx = (vx>maxspeed)?maxspeed:(vx+ax);
         }
@@ -314,14 +324,14 @@
 */
 void r_moveVertical(){
     if (flag_switch){
-        if (!switch1.read()){
+        if (!switch1.read() || emg1.read()){
             movedown = true;
             vy = (vy<-maxspeed)?-maxspeed:(vy-ay);
         }
         else {
             vy = movedown?0:vy;
         }
-        if (!switch2.read()){
+        if (!switch2.read() || emg2.read()){
             movedown = false;
             vy = (vy>maxspeed)?maxspeed:(vy+ay);
         }
@@ -342,10 +352,10 @@
 */
 void r_moveGripper(){
     if(flag_switch){
-        if(!switch1.read() && !gripperclosed){
+        if((!switch1.read() || emg1.read()) && !gripperclosed){
             gripperpwm.pulsewidth_us(1035);     // Close gripper
             gripperclosed = true;
-        } else if(!switch2.read() && gripperclosed){
+        } else if((!switch2.read() || emg2.read()) && gripperclosed){
             gripperpwm.pulsewidth_us(1500);     // Open gripper
             gripperclosed = false;
         }
@@ -456,9 +466,13 @@
 void r_processEMG(){
     if(flag_EMG){
         flag_EMG = false;
-//        emg1.tick();
-//        emg2.tick();
-//        emg3.tick();
+        emg1.tick();
+        emg2.tick();
+        scope.set(0, emg1.discrete);
+        scope.set(1, emg1.read()?0.5:0);
+        scope.set(2, emg2.discrete);
+        scope.set(3, emg2.read()?0.5:0);
+
     }
 }
 /*
@@ -497,9 +511,9 @@
 int main(){
     // Initialise main ticker
     mainticker.attach(&maintickerfunction,0.001f);
-
     robot_init();
     while(true){
+        unsigned int t1 = us_ticker_read();
         switch(state){
             case R_INIT:
                 break;
@@ -514,9 +528,16 @@
                 break;
         }
         r_processEMG();
+
+        if (flag_switch){
+            scope.send();
+
+        }
         r_processStateSwitch();
         r_switchFlagReset();
         r_doPID();
+        t1 = us_ticker_read()-t1;
+        scope.set(4, (float)t1);
         r_outputToMatlab();
     }
 }
\ No newline at end of file