Revision 23:58ec657a44f2, committed 2018-10-27

Comitter:

jvfausto

Date:

Sat Oct 27 16:47:50 2018 +0000

Parent:

22:d2f234fbc20d

Commit message:

With odometry

Changed in this revision

diff -r d2f234fbc20d -r 58ec657a44f2 wheelchair.cpp
--- a/wheelchair.cpp	Sat Oct 27 10:10:14 2018 +0000
+++ b/wheelchair.cpp	Sat Oct 27 16:47:50 2018 +0000
@@ -1,27 +1,33 @@
 #include "wheelchair.h"
-
+ 
 bool manual_drive = false;                                                             // Variable Changes between joystick and auto drive
 double curr_yaw, curr_vel, curr_velS;                                                             // Variable that contains current relative angle
 double encoder_distance;                                                               // Keeps distanse due to original position
-
+ 
 volatile double Setpoint, Output, Input, Input2;                                       // Variables for PID
 volatile double pid_yaw, Distance, Setpoint2, Output2, encoder_distance2;              // Variables for PID
 volatile double vIn, vOut, vDesired;
 volatile double vInS, vOutS, vDesiredS;
 volatile double yIn, yOut, yDesired;
 
+double z_angular, dist_old, curr_pos;
+double x_position = 0;
+double y_position = 0;
+ 
 PID myPID(&pid_yaw, &Output, &Setpoint, 5.5, .00, 0.0036, P_ON_E, DIRECT);             // Angle PID object constructor
 PID myPIDDistance(&Input, &Output, &Setpoint, 5.5, .00, 0.002, P_ON_E, DIRECT);       // Distance PID object constructor
 PID PIDVelosity(&vIn, &vOut, &vDesired, 5.5, .00, .002, P_ON_E, DIRECT);
 PID PIDSlaveV(&vInS, &vOutS, &vDesiredS, 5.5, .00, .002, P_ON_E, DIRECT);
 PID PIDAngularV(&yIn, &yOut, &yDesired, 5.5, .00, .002, P_ON_E, DIRECT);
-
+ 
 void Wheelchair::compass_thread() {                                                    // Thread that measures which angle we are at
      curr_yaw = imu->yaw();
+     z_angular = curr_yaw;
     }
 void Wheelchair::velosity_thread() {
     curr_vel = wheel->getVelosity();
      curr_velS = wheelS->getVelosity();
+     curr_pos = wheel->getDistance(53.975);
      }
     
 Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* qei, QEI* qeiS)   // Function Constructor for Wheelchair class
@@ -41,41 +47,41 @@
     ti = time;
     myPID.SetMode(AUTOMATIC);                                                           // set PID to automatic
 }
-
+ 
 void Wheelchair::move(float x_coor, float y_coor)                                       // moves the chair with joystick on manual
 {
-
+ 
     float scaled_x = ((x_coor * 1.6f) + 1.7f)/3.3f;                                     // Scales one joystic measurement to the
     float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f;                                     // chair's joystic measurement
     
     x->write(scaled_x);                                                                 // Sends the scaled joystic values to the chair
     y->write(scaled_y);
 }
-
+ 
 void Wheelchair::forward()                                                              // In auto to move foward
 {
     x->write(high);
     y->write(def+offset);
 }
-
+ 
 void Wheelchair::backward()                                                             // In auto to move reverse
 {
     x->write(low);
     y->write(def);
 }
-
+ 
 void Wheelchair::right()                                                                // In auto to move right
 {
     x->write(def);
     y->write(low);
 }
-
+ 
 void Wheelchair::left()                                                                 // In auto to move left
 {
     x->write(def);
     y->write(high);
 }
-
+ 
 void Wheelchair::stop()                                                                 // Stops the chair
 {
     x->write(def);
@@ -115,13 +121,13 @@
         
         out->printf("curr_yaw %f\r\r\n", curr_yaw);                              
         out->printf("Setpoint = %f \r\n", Setpoint);
-
+ 
         wait(.05);                                                                      // Small delay
         }
     Wheelchair::stop();                                                                 // Safety Stop
     out->printf("done \r\n");
 }
-
+ 
 void Wheelchair::pid_left(int deg)                                                      // Takes in degree and turns left
 {
     bool overturn = false;                                                              //Boolean if we have to turn under relative 0˚
@@ -154,19 +160,19 @@
         }
     Wheelchair::stop();                                                                 // Safety Stop
 }
-
+ 
 void Wheelchair::pid_turn(int deg) {                                                    // Determine wether we are turn right or left
-
+ 
     if(deg > 180) {                                                                     // If deg > 180 turn left: coterminal angle
         deg -= 360;
     }
-
+ 
     else if(deg < -180) {                                                               // If deg < -180 turn right: coterminal angle
         deg+=360;
     }  
     
     int turnAmt = abs(deg);                                                             // Makes sure input angle is positive
-
+ 
     if(deg >= 0){
         Wheelchair::pid_right(turnAmt);                                                 // Calls PID right if positive degree
     }
@@ -180,10 +186,10 @@
     Input = 0;                                                                          // Initializes imput to cero: Test latter w/o
     wheel->reset();                                                                     // Resets encoders so that they start at 0
     out->printf("pid foward\r\n");
-
+ 
     double tempor;                                                                      // Initializes Temporary variable for x input
     Setpoint = mm;                                                                      // Initializes the setpoint to desired value
-
+ 
     myPIDDistance.SetTunings(5.5,0, 0.0015);                                            // Sets constants for P and D
     myPIDDistance.SetOutputLimits(0,high-def-.15);                                      // Limits to the differnce between High and Def 
     myPIDDistance.SetControllerDirection(DIRECT);                                       // PID to Direct
@@ -197,7 +203,7 @@
         Input = wheel->getDistance(53.975);                                             // Gets Distance from Encoder onto PID
         wait(.05);                                                                      // Slight Delay: *****Test without
         myPIDDistance.Compute();                                                        // Compute Output for chair
-
+ 
         tempor = Output + def;                                                          // Temporary output variable                    
         x->write(tempor);                                                               // Sends to chair x output
         out->printf("distance %f\r\n", Input);
@@ -206,7 +212,7 @@
 }   
 void Wheelchair::pid_reverse(double mm)
 {
-
+ 
 }   
 void Wheelchair::pid_twistA()
 {
@@ -214,7 +220,7 @@
     double temporA = def;
     y->write(def); 
     x->write(def); 
-
+ 
     PIDAngularV.SetTunings(.00015,0, 0.00);                                                    // Sets the constants for P and D
     PIDAngularV.SetOutputLimits(-.1, .1);                                              // Limits to the differnce between def and low
     PIDAngularV.SetControllerDirection(DIRECT);                                               // PID mode Direct
@@ -244,7 +250,7 @@
         PIDAngularV.Compute();
         temporA += yOut;                                                     // Temporary value with the voltage output
         y->write(temporA); 
-        out->printf("temporA: %f, yDesired %f, angle: %f\r\n", temporA, yDesired, imu->gyro_z());
+        //out->printf("temporA: %f, yDesired %f, angle: %f\r\n", temporA, yDesired, imu->gyro_z());
         wait(.05);
     }
 }    
@@ -256,7 +262,7 @@
     char c;
     x->write(def);
     y->write(def);
-    
+    wheel->reset();
     PIDVelosity.SetTunings(.00005,0, 0.00);                                                    // Sets the constants for P and D
     PIDSlaveV.SetTunings(.007,0.000001, 0.000001);                                                    // Sets the constants for P and D
     PIDVelosity.SetOutputLimits(-.005, .005);                                              // Limits to the differnce between def and low
@@ -280,12 +286,13 @@
         if(c == 'r')
         {
             yDesired = 0;
+            dist_old = 0;
             return;
         }
         if(vDesired >= 0)
         {
-            PIDVelosity.SetTunings(.00001,0, 0.00);                                                    // Sets the constants for P and D
-            PIDVelosity.SetOutputLimits(-.003, .003);                                              // Limits to the differnce between def and low
+            PIDVelosity.SetTunings(.000004,0, 0.00);                                                    // Sets the constants for P and D
+            PIDVelosity.SetOutputLimits(-.002, .002);                                              // Limits to the differnce between def and low
         }
         else
         {
@@ -302,10 +309,25 @@
         temporS += vOutS;
         x->write(temporV);
         y->write(temporS);
-        out->printf("Velosity: %f, Velosity2: %f, temporV %f, temporS %f\r\n", curr_vel, curr_velS, temporV, temporS);
+        //out->printf("Velosity: %f, Velosity2: %f, temporV %f, temporS %f\r\n", curr_vel, curr_velS, temporV, temporS);
+        Wheelchair::odomMsg();
         wait(.01);
     }
 }
+void Wheelchair::odomMsg(){
+    double dist_new = curr_pos;
+    double dist = dist_new-dist_old;
+    double temp_x = dist*sin(z_angular*3.14159/180);
+    double temp_y = dist*cos(z_angular*3.14159/180);
+    
+    x_position += temp_x;
+    y_position += temp_y;
+    
+    dist_old = dist_new;
+ }
+ void Wheelchair::showOdom(){
+     out->printf("x %f, y %f, angle %f", x_position, y_position, z_angular);
+}
 float Wheelchair::getDistance() {
     return wheel->getDistance(Diameter);
     }
@@ -321,7 +343,7 @@
     Wheelchair::pid_right(87);
     Wheelchair::pid_forward(3658);
     }
-
+ 
 void Wheelchair::kitchen() {
     Wheelchair::pid_forward(5461);
     Wheelchair::pid_right(87);
@@ -329,9 +351,10 @@
     Wheelchair::pid_left(90);
     Wheelchair::pid_forward(305);
     }
-
+ 
 void Wheelchair::desk_to_kitchen(){
     Wheelchair::pid_right(180);
     Wheelchair::pid_forward(3700);
     }
-
+ 
+ 
\ No newline at end of file

diff -r d2f234fbc20d -r 58ec657a44f2 wheelchair.h
--- a/wheelchair.h	Sat Oct 27 10:10:14 2018 +0000
+++ b/wheelchair.h	Sat Oct 27 16:47:50 2018 +0000
@@ -82,10 +82,15 @@
     
     void pid_twistA();
     void pid_twistV();
+    
+    void odomMsg();
+    void showOdom();
+    
     /* functions with a predetermined path demmo*/
     void desk();
     void kitchen();
     void desk_to_kitchen();
+    
 private:
     /* Pointers for the joystick speed*/
     PwmOut* x;
@@ -96,6 +101,5 @@
     Timer* ti;                          // Pointer to the timer
     QEI* wheel;                         // Pointer to encoder
     QEI* wheelS;                         // Pointer to encoder
-
 };
 #endif
\ No newline at end of file