Project BioRobotics Group 19
Dependencies: FastPWM HIDScope MODSERIAL QEI biquadFilter mbed
Diff: main.cpp
- Revision:
- 19:6f720e4fcb47
- Parent:
- 18:2b6f41f39a7f
- Child:
- 20:ab391a133a01
--- a/main.cpp Thu Nov 02 16:23:26 2017 +0000 +++ b/main.cpp Fri Nov 03 09:47:09 2017 +0000 @@ -10,7 +10,7 @@ // SERIAL COMMUNICATION WITH PC //////////////////////////////////////////////// MODSERIAL pc(USBTX, USBRX); -HIDScope scope(4); +//HIDScope scope(4); // STATES ////////////////////////////////////////////////////////////////////// enum states{MOTORS_OFF, CALIBRATING, MOVING}; @@ -42,11 +42,11 @@ // MOTOR CONTROL /////////////////////////////////////////////////////////////// Ticker controllerTicker; // Ticker for the controller -const double controllerTs = 1/200.1; // Time step for controllerTicker [s] +const double controllerTs = 1/201.3; // Time step for controllerTicker [s] const double motorRatio = 131.25; // Ratio of the gearbox in the motors [] const double radPerPulse = 2*pi/(32*motorRatio); // Amount of radians the motor rotates per encoder pulse [rad/pulse] volatile double xVelocity = 0, yVelocity = 0; // X and Y velocities of the end effector at the start -const double velocity = 0.03; // X and Y velocity of the end effector when desired +const double velocity = 0.02; // X and Y velocity of the end effector when desired // MOTOR 1 volatile double position1; // Position of motor 1 [rad] @@ -83,7 +83,7 @@ // EMG ///////////////////////////////////////////////////////////////////////// Ticker emgLeft; // Ticker for EMG of left arm Ticker emgRight; // Ticker for EMG of right arm -const double emgTs = 0.491; // Time step for EMG sampling +const double emgTs = 0.4993; // Time step for EMG sampling // Filters BiQuadChain bqc; BiQuad bq2_high(0.875182, -1.750364, 0.87518, -1.73472, 0.766004); // High pass filter @@ -108,7 +108,7 @@ // PROCESS EMG SIGNALS ///////////////////////////////////////////////////////// Ticker processTicker; // Ticker for processing of EMG -const double processTs = 0.1; // Time step for processing of EMG +const double processTs = 0.101; // Time step for processing of EMG volatile bool xdir = true, ydir = false; // Direction the EMG signal moves the end effector volatile int count = 0; // Counter to change direction @@ -339,8 +339,8 @@ { if(currentState == MOVING) { - //position1 = radPerPulse * Encoder1.getPulses(); - //position2 = radPerPulse * Encoder2.getPulses(); + position1 = radPerPulse * Encoder1.getPulses(); + position2 = radPerPulse * Encoder2.getPulses(); if(active_l && active_r) // If both left and right EMG are active for 1 second the direction of control changes { @@ -551,7 +551,7 @@ bqc.add(&bq1_low ).add(&bq2_high ).add(&bq3_notch ); // Make BiQuad Chain processTicker.attach(&JointVelocities, processTs); // Ticker to process EMG - controllerTicker.attach(&MotorController, controllerTs); // Ticker to control motor 1 (PID) + controllerTicker.attach(&MotorController, controllerTs); // Ticker to control motor 1 (PID) emgRight.attach(&check_emg_r, emgTs); // Ticker to sample EMG of right arm emgLeft.attach(&check_emg_l, emgTs); // Ticker to sample EMG of left arm