Diff: main.cpp

Revision:

28:743485bb51e4

Parent:

27:d1814e271a95

Child:

29:948b0b14f6be

diff -r d1814e271a95 -r 743485bb51e4 main.cpp
--- 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)
 {