robot positie error test ding
Dependencies: MODSERIAL mbed EMG_Input QEI biquadFilter
Diff: main.cpp
- Revision:
- 12:e591729e854a
- Parent:
- 11:4382c567e0d4
- Child:
- 13:4f426186cd19
diff -r 4382c567e0d4 -r e591729e854a main.cpp --- 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