Diff: main.cpp

Revision:

53:bf0d97487e84

Parent:

52:8a8c53dc8547

Child:

54:4cda9af56bed

diff -r 8a8c53dc8547 -r bf0d97487e84 main.cpp
--- a/main.cpp	Tue Oct 27 09:32:06 2015 +0000
+++ b/main.cpp	Tue Oct 27 10:04:46 2015 +0000
@@ -48,6 +48,7 @@
 //Speed and Direction of motors - D4 (dir) and D5(speed) = motor 2, D7(dir) and D6(speed) = motor 1
 PwmOut pwm_motor1(D6);          //PWM motor 1
 PwmOut pwm_motor2(D5);          //PWM motor 2
+Servo servoPwm(D11);            //PWM servomotor     
 
 DigitalOut dir_motor1(D7);      //Direction motor 1
 DigitalOut dir_motor2(D4);      //Direction motor 2
@@ -285,6 +286,8 @@
     pc.baud(115200);            //serial baudrate
     red=1; green=1; blue=1;     //Make sure debug LEDs are off  
     
+    servoPwm.Enable(1650,20000); //Start position servo, PWM period in usecs
+    
     //theta1_cal = floor(theta1_lower * (4200/(2*PI)));
     //Encoder1.setPulses(theta1_cal);       //edited QEI library: added setPulses()
     
@@ -484,7 +487,6 @@
                       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();
@@ -899,22 +901,22 @@
     
     
     //Compare muscle amplitudes and determine their influence on x and y reference position.
-    if (biceps>triceps && biceps > 0.2){
+    if (biceps_avg>triceps_avg && biceps_avg > 0.2){
         xdir = 0;                            
-        xpower = biceps;}
-    else if (triceps>biceps && triceps>0.2){
+        xpower = biceps_avg;}
+    else if (triceps_avg>biceps_avg && triceps_avg>0.2){
         xdir = 1;
-        xpower = triceps;}
+        xpower = triceps_avg;}
     else 
         xpower=0;
         
-    if (flexor>extens && flexor > 0.2){
+    if (flexor_avg>extens_avg && flexor_avg > 0.2){
         ydir = 0;
-        ypower = flexor;
+        ypower = flexor_avg;
         }
-    else if (extens>flexor && extens > 0.2){
+    else if (extens_avg>flexor_avg && extens_avg > 0.2){
         ydir = 1;
-        ypower = extens;
+        ypower = extens_avg;
         }
     else
         ypower = 0;
@@ -1047,6 +1049,11 @@
     dir_motor2 = u2>0 ? 0 : 1;          //conditional statement, same as if else below
     pwm_motor2.write(abs(u2));
     
+    
+    //Servo alignment
+    theta3 = 180 - theta1 - theta2;
+    servoPwm.SetPosition = (2100/180)*theta3 + 600;
+    
     /*if(u1 > 0)
     {
         dir_motor1 = 0;
@@ -1135,7 +1142,7 @@
 void stop_control(void)
 {
     control_timer.detach();
-    
+    pwm_motor1=0; pwm_motor2=0;
     //Debug LED will be off when control is off
     blue=1;
     pc.printf("||- stopped control -|| \r\n");