groep 16
totale unit
Dependencies: mbed QEI HIDScope BiQuad4th_order biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 23:d13db573a875
- Parent:
- 22:08b3cd7bec7f
--- a/main.cpp Tue Oct 29 12:11:23 2019 +0000
+++ b/main.cpp Thu Oct 31 20:36:14 2019 +0000
@@ -4,68 +4,94 @@
#include "BiQuad.h"
#include "BiQuad4.h"
#include "MODSERIAL.h"
+#include "QEI.h"
+#include <vector>
+
+using std::vector;
Serial pc(USBTX,USBRX);
-DigitalIn button(SW3) ;
-
+DigitalIn button_motor(SW2) ;
+DigitalIn button_demo(D9) ;
+DigitalIn button_emg(D8);
+DigitalIn Fail_button(SW3);
+InterruptIn sw2(SW2);
+DigitalOut led_red(LED_RED);
+DigitalOut led_green(LED_GREEN);
+DigitalOut led_blue(LED_BLUE);
//Define objects
AnalogIn emg0( A0 );
AnalogIn emg1( A1 );
-AnalogIn potmeter1(PTC11); // Input of two potmeters
-AnalogIn potmeter2(PTC10);
+QEI Enc1(D12, D13, NC, 32);
+QEI Enc2(D10, D11, NC, 32);
+DigitalOut M1(D4);
+DigitalOut M2(D7);
+PwmOut E1(D5);
+PwmOut E2(D6);
-
+Ticker StateMachine;
Ticker ticker_calibration; // Ticker to send the EMG signals to screen
Ticker sample_timer; // Ticker for reading out EMG
HIDScope scope( 2 );
-DigitalOut led(LED1);
+DigitalOut led(LED_RED);
+DigitalOut controlLED(LED_GREEN);
+Timer timer;
volatile double emg1_filtered; //measured value of the first emg
volatile double emg2_filtered; //measured value of the second emg
-volatile double emg1_max ; // calibrated value of first emg
-volatile double emg2_max ;
volatile double emg1_cal = 0.8;
-
- // Read EMG
-//void EMGread()
-//{
-// emg1_filtered = FilterDesign(emg0.read());
-// emg2_filtered = FilterDesign(emg1.read());
- //pc.printf("emg1_cal = %f, emg2_cal = %f \n\r", emg1_filtered, emg2_filtered);
-//}
+double Pi = 3.14159265359;
+double gearRatio = 40/9;
+double time_in_state ;
+double initRot1 = 0;
+double initRot2 = -gearRatio*(180 - 48.5)/360;
+volatile double y_new = 20.0 ;
+volatile double y_prev = 20.0 ;
+double displacement = 1 ;
+double speedy = 3 ;
-
+enum states {WAITING, MOTOR_ANGLE_CLBRT, EMG_CLBRT, HOMING, WAITING4SIGNAL, MOVE_W_EMG, OPERATION, SHOOTING, MOVE_W_DEMO, FAILURE_MODE};
+states currentState = WAITING; // Start in waiting state
+bool stateChanged = true;
void sample() ;
-void EMGcalibration () ;
+void EMGcalibration() ;
+void moveMotorToPoint(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, double rotDes);
+double calcRot1(double xDes, double yDes);
+double calcRot2(double xDes, double yDes);
+void plotPath(double xStart, double yStart, double xEnd, double yEnd, double *xPath[], double *yPath[]);
+void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd, double speed);
+void moveMotorContinuously(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, vector<double> *pidInfo, Timer *t, double rotDes);
+double calcX(double rot1, double rot2);
+double calcY(double rot1, double rot2);
+void moveWithSpeed(double *xStart, double *yStart, double xSpeed, double ySpeed);
+void findDesiredLocation(double xStart, double yStart, double xEnd, double yEnd, double speed, Timer *t, vector<double> *desiredLocation);
+void DEMO_motor() ;
+void EMG_move();
+void state_machine(void);
+void moveMotorToStop(DigitalOut *M, PwmOut *E, QEI *Enc, double speed);
+void moveMotorsToStop(DigitalOut *M1, PwmOut *E1, QEI *Enc1, double speed1, DigitalOut *M2, PwmOut *E2, QEI *Enc2, double speed2);
-
+void changespeed()
+{
+ speedy = speedy*-1;
+}
int main()
-{
- /**Attach the 'sample' function to the timer 'sample_timer'.
- * this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz
- */
-
+{
+ led_red = 1;
+ led_green = 1;
+ led_blue = 1;
+
pc.baud(115200);
- //EMGcalibration();
- sample_timer.attach(&sample, 0.002);
-
-
- /*empty loop, sample() is executed periodically*/
+
+
+StateMachine.attach(&state_machine, 0.005f);
while(true) {
- if(SW3==0){
- EMGcalibration();
- }
-
- led = 1;
-
- while(emg1_filtered >= 0.9*emg1_cal){
- led = 0;
- }
+
+
}
}
@@ -74,14 +100,14 @@
-void sample()
+void sample()
{
emg1_filtered = FilterDesign(emg0.read());
emg2_filtered = FilterDesign(emg1.read());
/* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
scope.set(0, emg1_filtered ) ;
scope.set(1, emg2_filtered );
- /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
+ /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
* Ensure that enough channels are available (HIDScope scope( 2 ))
* Finally, send all channels to the PC at once */
scope.send();
@@ -90,15 +116,454 @@
//led = !led;
}
-void EMGcalibration ()
+void EMGcalibration()
+{
+
+ Timer tijd;
+ tijd.start();
+ do {
+ ticker_calibration.attach(&sample, 0.002);
+ if(emg1_cal < emg1_filtered) {
+ emg1_cal = emg1_filtered ;
+ pc.printf("EMG_cal : %g \n\r",emg1_cal);
+ }
+ } while(tijd<10);
+}
+
+
+//function to mave a motor to a certain number of rotations, counted from the start of the program.
+//parameters:
+//DigitalOut *M = pointer to direction cpntol pin of motor
+//PwmOut *E = pointer to speed contorl pin of motor
+//QEI *Enc = pointer to encoder of motor
+//double rotDes = desired rotation
+void moveMotorToPoint(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, double rotDes)
+{
+ double Kp = 30; //180 & 10 werkt zonder hulp
+ double Ki = 0;
+ double Kd = 2;
+
+ double pErrorC;
+ double pErrorP = 0;
+ double iError = 0;
+ double dError;
+
+ double U_p;
+ double U_i;
+ double U_d;
+
+ double rotC = Enc->getPulses()/(32*131.25) + initRot;
+ double MotorPWM;
+
+ Timer t;
+ double tieme = 0;
+
+ t.start();
+ do {
+ pErrorP = pErrorC;
+ rotC = Enc->getPulses()/(32*131.25) + initRot;
+ pErrorC = rotDes - rotC;
+ tieme = t.read();
+ t.reset();
+ iError = iError + pErrorC*tieme;
+ dError = (pErrorC - pErrorP)/tieme;
+
+ U_p = pErrorC*Kp;
+ U_i = iError*Ki;
+ U_d = dError*Kd;
+ MotorPWM = (U_p + U_i + U_d)*dir;
+
+ if(MotorPWM > 0) {
+ *M = 0;
+ *E = MotorPWM;
+ } else {
+ *M = 1;
+ *E = -MotorPWM;
+ }
+ wait(0.01);
+ //printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM);
+ } while (abs(MotorPWM) > 0.13|| abs(dError > 0.01));; //pErrorC > 0.02 || pErrorC < -0.02 ||dError > 0.01 || dError < -0.01);
+ *E = 0;
+ //pc.printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM);
+ t.stop();
+}
+
+
+void moveMotorContinuously(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, vector<double> *pidInfo, Timer *t, double rotDes)
+{
+ double Kp = 61; //180 & 10 werkt zonder hulp
+ double Ki = 1;
+ double Kd = 7;
+
+ double rotC = Enc->getPulses()/(32*131.25) + initRot;
+
+ double pErrorC = rotDes - rotC;
+
+ double tieme = t->read();
+ double dt = tieme - pidInfo->at(2);
+
+ double iError = pidInfo->at(1) + pErrorC*dt;
+ double dError = (pErrorC - pidInfo->at(0))/dt;
+
+ double MotorPWM = (pErrorC*Kp + iError*Ki + dError*Kd)*dir;
+
+ if(MotorPWM > 0) {
+ *M = 0;
+ *E = MotorPWM;
+ } else {
+ *M = 1;
+ *E = -MotorPWM;
+ }
+ pidInfo->clear();
+ pidInfo->push_back(pErrorC);
+ pidInfo->push_back(iError);
+ pidInfo->push_back(tieme);
+ pidInfo->push_back(Enc->getPulses()/(32*131.25) + initRot);
+}
+
+
+//double that calculates the rotation of one arm.
+//parameters:
+//double xDes = ofset of the arm's shaft in cm in the x direction
+//double yDes = ofset of the arm's shaft in cm in the y direction
+double calcRot1(double xDes, double yDes)
+{
+ double alpha = atan(yDes/xDes);
+ if(alpha < 0) {
+ alpha = alpha+Pi;
+ }
+ //pc.printf("alpha: %f", alpha);
+ return gearRatio*((alpha - 0.5*(Pi - acos((pow(xDes, 2.0) + pow(yDes, 2.0) - 2*pow(20.0, 2.0))/(-2*pow(20.0, 2.0)))))/(2*Pi));
+}
+
+double calcRot2(double xDes, double yDes)
+{
+ double alpha = atan(yDes/xDes);
+ if(alpha < 0) {
+ alpha = alpha+Pi;
+ }
+ return gearRatio*((alpha + 0.5*(Pi - acos((pow(xDes, 2.0) + pow(yDes, 2.0) - 2*pow(20.0, 2.0))/(-2*pow(20.0, 2.0)))))/(2*Pi));
+}
+
+void findDesiredLocation(double xStart, double yStart, double xEnd, double yEnd, double speed, Timer *t, vector<double> *desiredLocation)
+{
+ double pathLength = sqrt(pow((xStart - xEnd), 2.0)+pow((yStart - yEnd), 2.0));
+ double traveledDistance = speed * t->read();
+ double ratio = traveledDistance/pathLength;
+
+ desiredLocation->clear();
+ desiredLocation->push_back(xStart + (xEnd - xStart)*ratio);
+ desiredLocation->push_back(yStart + (yEnd - yStart)*ratio);
+
+}
+
+void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd, double speed)
+{
+ double rot1;
+ double rot2;
+
+ Timer t;
+
+ vector<double> desiredLocation;
+
+ vector<double> pidInfo1 (3);
+ vector<double> pidInfo2 (3);
+
+ fill(pidInfo1.begin(), pidInfo1.begin()+3, 0);
+ fill(pidInfo2.begin(), pidInfo2.begin()+3, 0);
+
+ double pathLength = sqrt(pow((xStart - xEnd), 2.0)+pow((yStart - yEnd), 2.0));
+
+ //Calculate the rotation of the motors at the start of the path
+ rot1 = calcRot1(xStart, yStart);
+ rot2 = calcRot2(xStart, yStart);
+ pc.printf("r1: %f r2: %f", rot1/6, rot2/6);
+
+ //Move arms to the start of the path
+ //moveMotorToPoint(&M1, &E1, &Enc1, initRot2, 1, -rot2);
+ //moveMotorToPoint(&M2, &E2, &Enc2, initRot1, -1, rot1);
+
+ //start the timer
+ t.start();
+ while(pathLength > speed * t.read()) {
+ findDesiredLocation(xStart, yStart, xEnd, yEnd, speed, &t, &desiredLocation);
+ rot1 = calcRot1(desiredLocation.at(0), desiredLocation.at(1));
+ //pc.printf("\n\r Rot1: %f", rot1);
+ moveMotorContinuously(&M1, &E1, &Enc1, initRot2, 1, &pidInfo2, &t, -rot2);
+ rot2 = calcRot2(desiredLocation.at(0), desiredLocation.at(1));
+ pc.printf("\n\r X: %f Y: %f Rot1: %f Rot2 %f", desiredLocation.at(0), desiredLocation.at(1), rot1, rot2);
+ moveMotorContinuously(&M2, &E2, &Enc2, initRot1, -1, &pidInfo1, &t, rot1);
+ wait(0.01);
+ }
+
+}
+
+
+
+double calcX(double rot1, double rot2)
+{
+ return 20*cos((rot1/gearRatio)*2*Pi)+20*cos((-rot2/gearRatio)*2*Pi);
+}
+
+double calcY(double rot1, double rot2)
+{
+ return 20*sin((rot1/gearRatio)*2*Pi)+20*sin((-rot2/gearRatio)*2*Pi);
+}
+
+
+void moveWithSpeed(double *xStart, double *yStart, double xSpeed, double ySpeed)
{
-
-Timer t;
-t.start();
- do {
- ticker_calibration.attach(&sample, 0.002);
- if(emg1_cal < emg1_filtered){
- emg1_cal = emg1_filtered ;
+ Timer t;
+
+ vector<double> pidInfo1 (4);
+ vector<double> pidInfo2 (4);
+
+ fill(pidInfo1.begin(), pidInfo1.begin()+4, 0);
+ fill(pidInfo2.begin(), pidInfo2.begin()+4, 0);
+
+ double xC = *xStart;
+ double yC = *yStart;
+
+ double rot1;
+ double rot2;
+
+ double tiemeP;
+ double tiemeC;
+ double dt;
+
+ t.start();
+
+ tiemeP = t.read();
+ while(t.read() < 0.1) {
+ tiemeC = t.read();
+ dt = tiemeC - tiemeP;
+ xC = xC+xSpeed*dt;
+ yC = yC+ySpeed*dt;
+ rot1 = calcRot1(xC, yC);
+ rot2 = calcRot2(xC, yC);
+ moveMotorContinuously(&M1, &E1, &Enc1, initRot2, 1, &pidInfo2, &t, -rot2);
+ moveMotorContinuously(&M2, &E2, &Enc2, initRot1, -1, &pidInfo1, &t, rot1);
+ tiemeP = tiemeC;
+ wait(0.01);
+ }
+
+ *xStart = xC;//calcX(pidInfo1.at(3), pidInfo2.at(3));
+ *yStart = yC;//calcY(pidInfo1.at(3), pidInfo2.at(3));
+}
+
+void moveMotorToStop(DigitalOut *M, PwmOut *E, QEI *Enc, double speed)
+{
+ Timer t;
+
+ double MotorPWM;
+
+ double posC;
+ double posP = Enc->getPulses()/(32*131.25);
+ double vel = 0;
+
+ int hasNotMoved = 0;
+
+
+ t.start();
+ do {
+ MotorPWM = speed - vel*0.7;
+ if(MotorPWM > 0) {
+ *M = 0;
+ *E = MotorPWM;
+ } else {
+ *M = 1;
+ *E = -MotorPWM;
+ }
+ wait(0.01);
+ posC = Enc->getPulses()/(32*131.25);
+ vel = (posC - posP)/t.read();
+ t.reset();
+ posP = posC;
+ pc.printf("v: %f hm: %d\n\r", MotorPWM, hasNotMoved);
+ if(abs(vel) > 0.001) {
+ hasNotMoved = 0;
+ } else {
+ hasNotMoved++;
+ }
+ } while(hasNotMoved < 6);
+ *E = 0;
+}
+
+void calibrateMotor()
+{
+ moveMotorToStop(&M1, &E1, &Enc1, -0.1);
+ moveMotorToStop(&M2, &E2, &Enc2, 0.2);
+ Enc2.reset();
+ moveMotorToStop(&M1, &E1, &Enc1, -0.1);
+ Enc1.reset();
+ pc.printf("%f", Enc1.getPulses());
+}
+
+void EMG_move()
+{
+
+ double xStart = 0;
+ double yStart = 20;
+
+ double rot1 = calcRot1(xStart, yStart);
+ double rot2 = calcRot2(xStart, yStart);
+
+ moveMotorToPoint(&M1, &E1, &Enc1, initRot2, 1, -rot2);
+ moveMotorToPoint(&M2, &E2, &Enc2, initRot1, -1, rot1);
+ sw2.rise(changespeed);
+ while(true) {
+
+
+ while(emg1_filtered >= 0.4*emg1_cal) {
+ //controlLED = !controlLED ;
+ moveWithSpeed(&xStart, &yStart, 0, speedy);
+ pc.printf("ystart: %g \n\r",yStart);
}
- }while(t<10);
-}
\ No newline at end of file
+
+ rot1 = calcRot1(xStart, yStart);
+ rot2 = calcRot2(xStart, yStart);
+ moveMotorToPoint(&M1, &E1, &Enc1, initRot2, 1, -rot2);
+ moveMotorToPoint(&M2, &E2, &Enc2, initRot1, -1, rot1);
+ }
+}
+
+void DEMO_motor()
+{
+ while(1){
+ moveAlongPath(10, 30, -10, 30, 1);
+ moveAlongPath(-10, 30, -10, 20, 1);
+ moveAlongPath(-10, 20, 10, 20, 1);
+ moveAlongPath(10, 20, 10, 30, 1);
+ }
+}
+
+
+
+
+
+void state_machine(void)
+{
+
+ switch(currentState) {
+
+ case WAITING:
+ // Description:
+ // In this state we do nothing, and wait for a command
+
+ // Actions
+ led_red = 0;
+ led_green = 0;
+ led_blue = 0; // Colouring the led WHITE
+
+ // State transition logic
+ if (button_motor < 1){
+ currentState = MOTOR_ANGLE_CLBRT;
+ stateChanged = true;
+ pc.printf("Starting Calibration\n\r");
+ } else if (Fail_button < 1){
+ currentState = FAILURE_MODE;
+ stateChanged = true;
+ }
+ break;
+
+ case MOTOR_ANGLE_CLBRT:
+ // Description:
+ // In this state the robot moves to a mechanical limit of motion,
+ // in order to calibrate the motors.
+ led_red = 1;
+ led_green = 0;
+ led_blue = 0;
+ timer.start();
+ if (stateChanged) {
+ calibrateMotor(); // Actuate motor 1 and 2.
+ stateChanged = true; // Keep this loop going, until the transition conditions are satisfied.
+ }
+ time_in_state = timer.read();
+ if(time_in_state>5) {
+ currentState = EMG_CLBRT ;
+ stateChanged = true;
+ pc.printf("Starting Calibration\n\r");
+ }
+
+ break;
+
+ case EMG_CLBRT:
+ led_red = 1;
+ led_green = 1;
+ led_blue = 0;
+ timer.start() ;
+ if (stateChanged) {
+ EMGcalibration();
+ stateChanged = true;
+ }
+ time_in_state = timer.read();
+ if (time_in_state >=5) {
+ currentState = WAITING4SIGNAL ;
+ stateChanged = true;
+ pc.printf("Waiting for signal");
+ break;
+
+ case WAITING4SIGNAL:
+ led_red = 1;
+ led_green = 0;
+ led_blue = 1;
+ if (button_motor < 1) {
+ currentState = MOTOR_ANGLE_CLBRT;
+ stateChanged = true;
+ pc.printf("Starting Calibration \n\r");
+ } else if (button_demo < 1) {
+ currentState = MOVE_W_DEMO;
+ stateChanged = true;
+ pc.printf("DEMO mode \r\n");
+ wait(1.0f);
+ } else if (button_emg <1) {
+ currentState = MOVE_W_EMG;
+ stateChanged = true;
+ pc.printf("EMG mode\r\n");
+ wait(1.0f);
+ } else if (Fail_button == 0) {
+ currentState = FAILURE_MODE;
+ stateChanged = true;
+ }
+
+ break;
+
+ case MOVE_W_EMG:
+ if (stateChanged) {
+ void EMG_move();
+ stateChanged = true;
+ }
+ if(button_demo < 1) {
+ currentState = WAITING4SIGNAL ;
+ stateChanged = true;
+ }
+
+ break;
+
+ case MOVE_W_DEMO:
+ if (stateChanged) {
+ DEMO_motor() ;
+ stateChanged = true;
+ }
+ if(button_demo < 1) {
+ currentState = WAITING4SIGNAL ;
+ stateChanged = true;
+ }
+
+ break;
+
+ case FAILURE_MODE:
+ if (stateChanged) {
+ led_red = 0;
+ led_green = 1;
+ led_blue = 1;
+ stateChanged = true;
+ }
+ break;
+
+
+
+
+}
+}
+}