Odometry communication
Dependencies: PID QEI chair_BNO055 ros_lib_kinetic3
Dependents: wheelchaircontrolrealtime1
Fork of wheelchaircontrol by
wheelchair.cpp
- Committer:
- jvfausto
- Date:
- 2018-10-16
- Revision:
- 21:8d11d953ceeb
- Parent:
- 20:f42db4ae16f0
- Child:
- 22:d2f234fbc20d
File content as of revision 21:8d11d953ceeb:
#include "wheelchair.h" bool manual_drive = false; // Variable Changes between joystick and auto drive double curr_yaw; // 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 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 void Wheelchair::compass_thread() { // Thread that measures which angle we are at curr_yaw = imu->yaw(); } Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* qei) // Function Constructor for Wheelchair class { //Initializes X and Y variables to Pins x = new PwmOut(xPin); y = new PwmOut(yPin); // Initializes IMU Library imu = new chair_BNO055(pc, time); Wheelchair::stop(); // Wheelchair is not moving when initializing imu->setup(); // turns on the IMU out = pc; // "out" is called for serial monitor wheel = qei; // "wheel" is called for encoder out->printf("wheelchair setup done \r\n"); // make sure it initialized 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); y->write(def); } // counter clockwise is - // clockwise is + void Wheelchair::pid_right(int deg) // Takes in degree and turns right { bool overturn = false; //Boolean if we have to turn over relative 360˚ out->printf("pid right\r\r\n"); x->write(def); // Not moving fowards or reverse Setpoint = curr_yaw + deg; // Relative angle we want to turn pid_yaw = curr_yaw; // Sets input to current angle(pid_yaw = input) if(Setpoint > 360) { //Turns on overturn boolean if setpoint over 360˚ overturn = true; } myPID.SetTunings(5.5,0, 0.0035); // Sets the constants for P and D myPID.SetOutputLimits(0, def-low-.15); // Limits to the differnce between def and low myPID.SetControllerDirection(DIRECT); // PID mode Direct while(pid_yaw < Setpoint - 3){ // Tells PID to stop when reaching // a little less than desired angle if(overturn && curr_yaw < Setpoint-deg-1) // Sets PID yaw to coterminal angle if necesary { pid_yaw = curr_yaw + 360; } else pid_yaw = curr_yaw; myPID.Compute(); // Does PID calculations double tempor = -Output+def; // Temporary value with the voltage output y->write(tempor); // Sends to chair y output command 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˚ out->printf("pid Left\r\r\n"); x->write(def); // Not moving fowards or reverse Setpoint = curr_yaw - deg; // Relative angle we want to turn pid_yaw = curr_yaw; // Sets input to current angle(pid_yaw = input) if(Setpoint < 0) { //Turns on overturn boolean if setpoint under 0˚ overturn = true; } myPID.SetTunings(5,0, 0.004); // Sets the constants for P and D myPID.SetOutputLimits(0,high-def-.12); // Limits to the differnce between High and Def myPID.SetControllerDirection(REVERSE); // PID mode Reverse while(pid_yaw > Setpoint+3){ // Tells PID to stop when reaching // a little more than desired angle if(overturn && curr_yaw > Setpoint+deg+1) // Sets PID yaw to coterminal angle if necesary { pid_yaw = curr_yaw - 360; } else pid_yaw = curr_yaw; myPID.Compute(); // Does PID calculations double tempor = Output+def; // Temporary value with the voltage output y->write(tempor); // Sends to chair y output command out->printf("curr_yaw %f\r\n", curr_yaw); wait(.05); // Small Delay } 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 } else { Wheelchair::pid_left(turnAmt); // Calls PID left if negative degree } } void Wheelchair::pid_forward(double mm) { mm -= 20; // Makes sure distance does not overshoot 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 y->write(def+offset); // Sets chair to not turn while(Input < Setpoint){ // Stop moving when reaching setpoint if(out->readable()) // Emergency Break break; 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); } } void Wheelchair::pid_reverse(double mm) { } float Wheelchair::getDistance() { return wheel->getDistance(Diameter); } void Wheelchair::resetDistance(){ wheel->reset(); } /*Predetermined paths For Demmo*/ void Wheelchair::desk() { Wheelchair::pid_forward(5461); Wheelchair::pid_right(87); Wheelchair::pid_forward(3658); Wheelchair::pid_right(87); Wheelchair::pid_forward(3658); } void Wheelchair::kitchen() { Wheelchair::pid_forward(5461); Wheelchair::pid_right(87); Wheelchair::pid_forward(3658); Wheelchair::pid_left(90); Wheelchair::pid_forward(305); } void Wheelchair::desk_to_kitchen(){ Wheelchair::pid_right(180); Wheelchair::pid_forward(3700); }