2nd draft
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed Servo
Fork of robot_mockup by
Diff: main.cpp
- Revision:
- 28:743485bb51e4
- Parent:
- 27:d1814e271a95
- Child:
- 29:948b0b14f6be
--- a/main.cpp Mon Oct 19 08:34:24 2015 +0000 +++ b/main.cpp Mon Oct 19 10:23:56 2015 +0000 @@ -53,8 +53,8 @@ DigitalOut dir_motor2(D4); //Direction motor 2 //Limit Switches -InterruptIn shoulder_limit(D3); //using FRDM buttons -InterruptIn elbow_limit(D4); //using FRDM buttons +InterruptIn shoulder_limit(D2); //using FRDM buttons +InterruptIn elbow_limit(D3); //using FRDM buttons //Other buttons DigitalIn button1(PTA4); //using FRDM buttons @@ -95,10 +95,10 @@ //PID variables double u1; double u2; //Output of PID controller (PWM value for motor 1 and 2) double m1_error=0; double m1_e_int=0; double m1_e_prev=0; //Error, integrated error, previous error -const double m1_kp=0; const double m1_ki=0; const double m1_kd=0; //Proportional, integral and derivative gains. +const double m1_kp=1; const double m1_ki=0.0125; const double m1_kd=0.1; //Proportional, integral and derivative gains. double m2_error=0; double m2_e_int=0; double m2_e_prev=0; //Error, integrated error, previous error -const double m2_kp=0; const double m2_ki=0; const double m2_kd=0; //Proportional, integral and derivative gains. +const double m2_kp=1; const double m2_ki=0.0125; const double m2_kd=0.1; //Proportional, integral and derivative gains. //highpass filter 20 Hz const double high_b0 = 0.956543225556877; @@ -148,6 +148,16 @@ const double low_a1 = -1.968902268531908; const double low_a2 = 0.9693784555043481; +//Forward Kinematics +const double l1 = 0.25; const double l2 = 0.25; +double current_x; double current_y; +double theta1; double theta2; +double dtheta1; double dtheta2; +double rpc; +double x; double y; +double x_error; double y_error; +double costheta1; double sintheta1; +double costheta2; double sintheta2; /*-------------------------------------------------------------------------------------------------------------------- ---- Filters --------------------------------------------------------------------------------------------------------- @@ -205,7 +215,11 @@ //Menu functions void mainMenu(); void caliMenu(); +void controlMenu(); +void controlMenu2(); void clearTerminal(); +void emgInstructions(); +void titleBox(); /*-------------------------------------------------------------------------------------------------------------------- @@ -218,28 +232,133 @@ red=1; green=1; blue=1; //Make sure debug LEDS are off //Set PwmOut frequency to 10k Hz - pwm_motor1.period(PWM_PERIOD); - pwm_motor2.period(PWM_PERIOD); + //pwm_motor1.period(PWM_PERIOD); + //pwm_motor2.period(PWM_PERIOD); clearTerminal(); //Clear the putty window // make a menu, user has to initiate next step + titleBox(); mainMenu(); //caliMenu(); // Menu function //calibrate_arm(); //Start Calibration //calibrate_emg(); - wait(3); - start_control(); //100 Hz control + + + x=0; y=0.5; + //start_control(); //100 Hz control //maybe some stop commands when a button is pressed: detach from timers. //stop_control(); //stop_sampling(); - char c; - pc.printf("entering while loop \r\n"); + //char c; + + + + char command; - while(1) { + while(command != 'Q' && command != 'q') + { + if(pc.readable()){ + command = pc.getc(); + + switch(command){ + + case 'c': + case 'C': + pc.printf("\n\r => You chose calibration.\r\n\n"); + caliMenu(); + + char command2=0; + + while(command2 != 'B' && command2 != 'b'){ + command2 = pc.getc(); + switch(command2){ + case 'a': + case 'A': + pc.printf("\n\r => Arm Calibration Starting... please wait \n\r"); + calibrate_arm(); + wait(1); + caliMenu(); + break; + + case 'e': + case 'E': + pc.printf("\n\r => EMG Calibration Starting... please wait \n\r"); + wait(1); + emgInstructions(); + calibrate_emg(); + pc.printf("\n\r ------------------------ \n\r"); + pc.printf("\n\r EMG Calibration complete \n\r"); + pc.printf("\n\r ------------------------ \n\r"); + caliMenu(); + break; + + case 'b': + case 'B': + pc.printf("\n\r => Going back to main menu.. \n\r"); + mainMenu(); + break; + }//end switch + + }//end while + break; + case 's': + case 'S': + pc.printf("=> You chose control \r\n\n"); + wait(1); + start_sampling(); + wait(1); + start_control(); + wait(1); + controlMenu2(); + break; + case 'R': + red=!red; + pc.printf("=> Red LED triggered \n\r"); + break; + case 'G': + green=!green; + pc.printf("=> Green LED triggered \n\r"); + break; + case 'B': + blue=!blue; + pc.printf("=> Blue LED triggered \n\r"); + break; + case 'q': + case 'Q': + + break; + default: + pc.printf("=> Invalid Input \n\r"); + break; + } //end switch + } // end if pc readable + + } // end while loop + + + + //When end of while loop reached (user chose quit program) - detach all timers and stop motors. + + pc.printf("\r\n------------------------------ \n\r"); + pc.printf("Program Offline \n\r"); + pc.printf("Reset to start\r\n"); + pc.printf("------------------------------ \n\r"); +} +//end of main + +/*-------------------------------------------------------------------------------------------------------------------- +---- FUNCTIONS ------------------------------------------------------------------------------------------------------- +--------------------------------------------------------------------------------------------------------------------*/ + +void controlMenu2() +{ + controlMenu(); + char c=0; + while(c != 'Q' && c != 'q') { if( pc.readable() ){ c = pc.getc(); @@ -271,10 +390,19 @@ break; case 'q' : - pc.printf("Quit"); + case 'Q' : + pc.printf("=> Quitting control... \r\n"); wait(1); + stop_sampling(); + stop_control(); + pwm_motor1=0; pwm_motor2=0; + pc.printf("Sampling and Control detached \n\r"); wait(1); + pc.printf("Returning to Main Menu \r\n\n"); wait(1); + mainMenu(); + //running = false; break; }//end switch + if(c!='q' && c!='Q'){ pc.printf("Reference position: %f and %f \r\n",x,y); pc.printf("Current position: %f and %f \r\n",current_x,current_y); pc.printf("Current angles: %f and %f \r\n",theta1,theta2); @@ -282,15 +410,11 @@ pc.printf("PID output: %f and %f \r\n",u1,u2); pc.printf("----------------------------------------\r\n\n"); } + } //end if } //end of while loop -} -//end of main - -/*-------------------------------------------------------------------------------------------------------------------- ----- FUNCTIONS ------------------------------------------------------------------------------------------------------- ---------------------------------------------------------------------------------------------------------------------*/ + } //Sample and Filter void sample_filter(void) @@ -329,6 +453,23 @@ void controlMenu() { + //Title Box + pc.putc(201); + for(int j=0;j<33;j++){ + pc.putc(205); + } + pc.putc(187); + pc.printf("\n\r"); + pc.putc(186); pc.printf(" Control Menu "); pc.putc(186); + pc.printf("\n\r"); + pc.putc(200); + for(int k=0;k<33;k++){ + pc.putc(205); + } + pc.putc(188); + + pc.printf("\n\r"); + //endbox pc.printf("1) Move Arm Left\r\n"); pc.printf("2) Move Arm Right\r\n"); pc.printf("3) Move Arm Up\r\n"); @@ -340,7 +481,7 @@ //Send arm to mechanical limits, and set encoder to 0. Then send arm to starting position. void calibrate_arm(void) { - pc.printf("Calibrate_arm() entered\r\n"); + pc.printf("Calibrate_arm() entered\r\n"); bool calibrating = true; bool done1 = false; bool done2 = false; @@ -385,7 +526,9 @@ //Encoder2.setPulses(100); //edited QEI library: added setPulses() //pc.printf("Elbow Limit hit - shutting down motor 2. Current pulsecount: %i \r\n",Encoder1.getPulses()); wait(1); + pc.printf("\n\r ------------------------ \n\r"); pc.printf("Arm Calibration Complete\r\n"); + pc.printf(" ------------------------ \n\r"); } @@ -457,7 +600,10 @@ pc.printf("+ means current sample is higher than stored MVC\r\n"); pc.printf("- means current sample is lower than stored MVC\r\n"); wait(2); - pc.printf(" Biceps is first. "); wait(1); + pc.printf("\r\n----------------\r\n "); + pc.printf(" Biceps is first.\r\n "); + pc.printf("----------------\r\n "); + wait(1); pc.printf(" Press any key to begin... "); wait(1); char input; input=pc.getc(); @@ -480,7 +626,11 @@ // Triceps: muscle=2; - pc.printf(" Triceps is next "); wait(1); + pc.printf("\r\n----------------\r\n "); + pc.printf(" Triceps is next.\r\n "); + pc.printf("----------------\r\n "); + wait(1); + pc.printf(" Press any key to begin... "); wait(1); input=pc.getc(); pc.putc(input); @@ -611,16 +761,14 @@ void mainMenu() { - //Title Box + //Title Box pc.putc(201); for(int j=0;j<33;j++){ pc.putc(205); } pc.putc(187); pc.printf("\n\r"); - pc.putc(186); pc.printf(" BioRobotics M9 - Group 14 "); pc.putc(186); - pc.printf("\n\r"); - pc.putc(186); pc.printf(" Robot powered ON "); pc.putc(186); + pc.putc(186); pc.printf(" Main Menu "); pc.putc(186); pc.printf("\n\r"); pc.putc(200); for(int k=0;k<33;k++){ @@ -630,8 +778,15 @@ pc.printf("\n\r"); //endbox + wait(1); + pc.printf("[C]alibration\r\n"); wait(0.2); + pc.printf("[S]tart Control with buttons\r\n"); wait(0.2); + pc.printf("[Q]uit Robot Program\r\n"); wait(0.2); + pc.printf("[R]ed LED\r\n"); wait(0.2); + pc.printf("[G]reen LED\r\n"); wait(0.2); + pc.printf("[B]lue LED\r\n"); wait(0.2); + pc.printf("Please make a choice => \r\n"); } -void caliMenu(){}; //Start sampling void start_sampling(void) @@ -688,6 +843,65 @@ pc.printf("[H"); // cursor to home } +void caliMenu(){ + + //Title Box + pc.putc(201); + for(int j=0;j<33;j++){ + pc.putc(205); + } + pc.putc(187); + pc.printf("\n\r"); + pc.putc(186); pc.printf(" Calibration Menu "); pc.putc(186); + pc.printf("\n\r"); + pc.putc(200); + for(int k=0;k<33;k++){ + pc.putc(205); + } + pc.putc(188); + + pc.printf("\n\r"); + //endbox + + pc.printf("[A]rm Calibration\r\n"); + pc.printf("[E]MG Calibration\r\n"); + pc.printf("[B]ack to main menu\r\n"); + pc.printf("Please make a choice => \r\n"); + +} + +void titleBox(){ + + //Title Box + pc.putc(201); + for(int j=0;j<33;j++){ + pc.putc(205); + } + pc.putc(187); + pc.printf("\n\r"); + pc.putc(186); pc.printf(" BioRobotics M9 - Group 14 "); pc.putc(186); + pc.printf("\n\r"); + pc.putc(186); pc.printf(" Robot powered ON "); pc.putc(186); + pc.printf("\n\r"); + pc.putc(200); + for(int k=0;k<33;k++){ + pc.putc(205); + } + pc.putc(188); + + pc.printf("\n\r"); + //endbox +} + +void emgInstructions(){ + pc.printf("\r\nPrepare the skin before applying electrodes: \n\r"); wait(1); + pc.printf("-> Shave electrode locations if needed and clean with alcohol \n\r"); wait(1); + pc.printf("\n\r Relax for a few minutes after electrodes are placed and check if EMG signal looks normal \n\r "); wait(1); + pc.printf("\n\r To calibrate the EMG signals we will measure the Maximum Voluntary Contraction of each muscle \n\r"); wait(1); + pc.printf("You will need to flex the mentioned muscle as much as possible for 5 seconds \n\r"); wait(1); + pc.printf("The measurement will begin once you confirm you're ready [Hit any key] \n\r \n\r"); wait(1); +} + //keeps input limited between min max void keep_in_range(double * in, double min, double max) {