acos faalt
Dependencies: HIDScope MODSERIAL QEI mbed
Revision 6:05b6b70618db, committed 2015-10-21
- Comitter:
- davevogel0
- Date:
- Wed Oct 21 21:00:48 2015 +0000
- Parent:
- 5:ec6dd614aa7e
- Commit message:
- Using the potmeter an angle is send out to the motors with a PID control.
Changed in this revision
Servo.lib | Show diff for this revision Revisions of this file |
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r ec6dd614aa7e -r 05b6b70618db Servo.lib --- a/Servo.lib Wed Oct 21 13:51:43 2015 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/users/jdenkers/code/Servo/#352133517ccc
diff -r ec6dd614aa7e -r 05b6b70618db main.cpp --- a/main.cpp Wed Oct 21 13:51:43 2015 +0000 +++ b/main.cpp Wed Oct 21 21:00:48 2015 +0000 @@ -2,15 +2,6 @@ #include "HIDScope.h" #include "MODSERIAL.h" #include "QEI.h" -#include "Servo.h" - -//Aray of locations and Length of parts -float X [10] = {-0.4, -0.3, -0.2, -0.1, 0, 0.1, 0.2, 0.3, 0.4, 0.5}; -float Y [10] = {-0.4, -0.3, -0.2, -0.1, 0, 0.1, 0.2, 0.3, 0.4, 0.5}; -const double L1 = 0.34; -const double L2 = 0.26; -const double L_board= 0.42; -const double R_Max_error = 0.01; //---------- Change motor control parameters------- //Motor 1 @@ -18,13 +9,13 @@ const float M1_Kp = 0.1; //strentgh of proportional control const float M1_Kd = 0.1; //strentgh of differential control Include timestep in constant const float M1_Ki = 0.1; //strentgh of integrational control Include timestep in constant -const double M1_friction = 0.55; //minimum power required to make the motor move +const double M1_friction = 0.4; //minimum power required to make the motor move //Motor 2 -const float M2_Ks = 0.4; //minimum power required to move -const float M2_Kp = -2; //strentgh of proportional control -const float M2_Kd = 0.1; //strentgh of differential control Include timestep in constant -const float M2_Ki = 0.1; //strentgh of integrational control Include timestep in constant -const double M2_friction = 0.55; //minimum power required to make the motor move +const float M2_Ks = 0; //minimum power required to move +const float M2_Kp = -10; //strentgh of proportional control +const float M2_Kd = 0; //strentgh of differential control Include timestep in constant +const float M2_Ki = 1; //strentgh of integrational control Include timestep in constant +const double M2_friction = 0.4; //minimum power required to make the motor move //----------End of control parameters //encoder @@ -39,35 +30,23 @@ AnalogIn pot1(A0); //read value of pot1 for position AnalogIn pot2(A1); //read value of pot2 for position DigitalOut myled(LED_GREEN); -DigitalOut magnet(D8); //magnet is defined on D8 MODSERIAL pc(USBTX, USBRX); DigitalIn button(PTA4); -DigitalIn button_Arm1(D1); -DigitalIn button_Arm2(D2); -DigitalIn button_Servo(D0); HIDScope scope(2); -Servo swagvo(D3); //Define Variables -double M1_dpos, M1_pos; -double M2_dpos, M2_pos; -double M_error, M1_error, M2_error; -double R_set, A_R, R_error, R_error_int, R_cal, R_cal1, R_cal2; -double Q_set, Q_end, Q_error, Q_error_int, Q_cal, Q_cal1, Q_cal2; +double M1_set, M1_error, M1_pos, M1_error_int, M1_error1; +double M2_set, M2_error, M2_pos, M2_error_int, M2_error1; double position; const double long gearbox = 0.085877862; -const double long PI = 3.14159265359; -double M_error1 = 0; -int run = 0; -bool H_wheel=0; // 0 is up and 1 is down -bool Magnet=0; + //Timers and Tickers Timer t; Ticker t1,t2,t3; //booleans run program -volatile bool send_go = false, setpoint_go = false, control_go= false, calibration_go= false; +volatile bool send_go = false, setpoint_go = false, control_go= false; //------------------Activate programs----------- //Activate send data pc @@ -89,7 +68,6 @@ //------------------Start of control programs------- - //Send values over HIDScope & Serial port void Send() { @@ -99,61 +77,36 @@ } //Desired Position Motors -void Setpoint() +void M_Setpoint() { - // check if - double R_set1=R_set; - double Q_set1=Q_set; - - //set desired x and y point in array --> do this with EMG - int dX = pot1.read() * 10; - int dY = pot2.read() * 10; + M1_set=pot1.read()*360-180; + M2_set=pot2.read()*360-180; +} - //Set the length of the arm - R_set = sqrt( X[dX] * X[dX] + Y[dY] * Y[dY]); //desired length of the arm - Q_set = acos( Y[dY] / X[dX] ); //desired angle of the arm (theta for Qref) [rad] - - if (R_set1!=R_set && Q_set1 != Q_set) { // of ipv en - pc.printf("reset\n"); - run=run+1; - R_error_int=0; - Q_error_int=0; - } - +// read position of motors +void M_pos () +{ + M1_pos = gearbox * Motor1.getPulses(); + M2_pos = gearbox * Motor1.getPulses(); } -void Lengtharm() +// calculate error between pos and setpoint +void M_error () { - //read out length of arm - double A_Rx = L1 * cos(gearbox * Motor1.getPulses() * PI / 180.0) + L2 * cos( (gearbox * Motor1.getPulses() + gearbox * Motor2.getPulses())* PI / 180.0); - double A_Ry = L1 * sin(gearbox * Motor1.getPulses() * PI / 180.0) + L2 * sin( (gearbox * Motor1.getPulses() + gearbox * Motor2.getPulses())* PI / 180.0); - A_R = sqrt( pow (A_Rx, 2) + pow (A_Ry, 2)); //length Arm - R_error= A_R-R_set; //error in arm length + M1_error= M1_set - M1_pos; + M2_error= M2_set - M2_pos; } -void Qpos() -{ - //read out pos of Motor 1 - double A_Rx = L1 * cos(gearbox * Motor1.getPulses() * PI / 180.0) + L2 * cos( (gearbox * Motor1.getPulses() + gearbox * Motor2.getPulses())* PI / 180.0); - double A_Ry = L1 * sin(gearbox * Motor1.getPulses() * PI / 180.0) + L2 * sin( (gearbox * Motor1.getPulses() + gearbox * Motor2.getPulses())* PI / 180.0); - pc.printf("A_Rx = %f A_Ry = %f", A_Rx, A_Ry); - Q_end= acos( A_Ry / A_Rx ); //actual angle of the arm //Aandacht is dit graden(moet in radians)?!!!!! - pc.printf("Q_set = %f Q_end= %f \n", Q_set, Q_end); - Q_error= Q_end-Q_set; //error in angle of Qref -} - -double Pw_control (double S_error, double& S_error_int, double Ks, double Kp,double Ki,double Kd, double friction) +//translate error to power +double Pw_control (double& S_error, double& S_error_int, double S_error1, double Ks, double Kp,double Ki,double Kd, double friction) { // Motor Power - M_error = S_error; //give the amount of error - - double M_error_int = S_error_int + M_error / 1e4; - S_error_int = M_error_int; + S_error_int = S_error_int + S_error / 1e4; double Ps = Ks; - double Pp = Kp * M_error; //Proportional control - double Pi = Ki * M_error_int; //check dit --> nog niet goed //int controll - double Pd = Kd * (M_error - M_error1); //Differtial control - M_error1 = M_error; + double Pp = Kp * S_error; //Proportional control + double Pi = Ki * S_error_int; //int controll + double Pd = Kd * (S_error - S_error1); //Differtial control + S_error1 = S_error; double Power = Ps + Pp + Pi + Pd; pc.printf("Power bf: %f Pi: %f \n", Power, Pi); @@ -164,27 +117,17 @@ Power=0.7; } else {} - return Power ;//is it possible to return 2 things for -} - -// direction controll -bool dr_control (double A, double B) -{ - // Direction motor should turn - int Direction = (A > B) ? true:false; - return Direction; + return Power ; } -void Servo() +// direction control +bool dr_control (double A, double B) { - swagvo.SetPosition(650); //lower wire - wait(1); - magnet = !magnet; //attach piece - wait(1); - swagvo.SetPosition(2350); //raise wire + int Direction = (A > B) ? false:true; + return Direction; } +//--------------- End of control programs---------- -//--------------- End of control programs---------- int main() { //turn that led off!It hurts my eyes! Ow, I do boot. @@ -193,122 +136,35 @@ //PWM period motors E1.period(0.0001f); E2.period(0.0001f); - swagvo.Enable(1500,20000); - pc.baud(115200); //sub programs - time how fast everything occurs - t1.attach_us(&Send_True, 1e6); //Send data to pc - t2.attach_us(&M_Setpoint_True, 1e6); //Desired position motor(EMG goes here) - t3.attach_us(M_Control_True, 1e6); //Speed control here - -//calibration -// 0 point position - pc.printf("0 cal\n"); - bool cal0 = true; - while (cal0 == true) { - if (button == false) { - Qpos(); - Lengtharm(); - Q_cal=Q_end; - R_cal=A_R; - cal0= false; - - //show usefull info on serial - pc.printf("Q_set = %f Q_end= %f Q_error = %f \n", Q_set, Q_end, Q_error); - pc.printf("R_set = %f A_R = %f R_error = %f \n", R_set, A_R, R_error); - } - } - - - // Board point A - wait (0.5); - pc.printf("A cal\n"); - bool calA = true; - while (calA == true) { - if (button == false) { - Qpos(); - Lengtharm(); - Q_cal1=Q_end; - R_cal1=A_R; - calA = false; - - //show usefull info on serial - pc.printf("Q_set = %f Q_end= %f Q_error = %f \n", Q_set, Q_end, Q_error); - pc.printf("R_set = %f A_R = %f R_error = %f \n", R_set, A_R, R_error); - } - } - - //Board point B - - wait (0.5); - pc.printf("B cal\n"); - bool calB = true; - while (calB == true) { - if (button == false) { - Qpos(); - Lengtharm(); - Q_cal2=Q_end; - R_cal2=A_R; - calB = false; - - //show usefull info on serial - pc.printf("Q_set = %f Q_end= %f Q_error = %f \n", Q_set, Q_end, Q_error); - pc.printf("R_set = %f A_R = %f R_error = %f \n", R_set, A_R, R_error); - } - } - + t1.attach_us(&Send_True, 1e4); //Send data to pc + t2.attach_us(&M_Setpoint_True, 1e4); //Desired position motor + t3.attach_us(M_Control_True, 1e4); //Speed control here //-------------Schedule programs------------------- while(1) { - if (calibration_go == true) { - pc.printf("calibration R_set = %f R_cal = %f \n", R_set, R_cal); - //setpoint - R_set = R_cal; - Q_set = Q_cal; - if (button_Arm1 == true && button_Arm2 == true ) { - pc.printf("cal is set\n"); - Q_end = Q_cal; - A_R = R_cal; - calibration_go == false; - } else {} - } if(setpoint_go == true && calibration_go == false) { + + if(setpoint_go == true) { + M_Setpoint(); pc.printf("setpoint\n"); - if (run == 2) { - pc.printf("Run = %f\n", run); - calibration_go == true; - run = 0; - } else { - Setpoint(); - } + setpoint_go = false; } if (control_go == true) { - pc.printf("control\n"); - Lengtharm(); - Qpos(); - if (-1*R_Max_error<R_error<R_Max_error) { - pc.printf("Length control R_error = %f R_set = %f A_R = %f\n", R_error, R_set, A_R); - // control Motor2 - E2 = Pw_control (R_error, R_error_int, M2_Ks, M2_Kp, M2_Ki, M2_Kd, M2_friction); - M2 = dr_control (A_R,R_set); - M2_error = M_error; - //print error - pc.printf("R_error = %f\n", R_error); - } else if (-1*R_Max_error<Q_error<R_Max_error) { - pc.printf("Angle control Q_error = %f Q_set = %f Q_end = %f\n", Q_error, Q_set, Q_end); - // control Motor 1 - E1 = 0;//Pw_control (Q_error, Q_error_int, M1_Ks, M1_Kp, M1_Ki, M1_Kd, M1_friction); - M1 = dr_control (Q_end, Q_set); - // control Motor 2 - has to do nothing - E2 = 0; - // print error - pc.printf("Q_error = %f\n", Q_error); - } else { - E1 = 0; - E2 = 0; - pc.printf("i dont know what to do R_error = %f R_set = %f A_R = %f\n", R_error, R_set, A_R); - } + pc.printf("control:"); + M_pos(); + M_error(); + //control Motor 1 + E1 = Pw_control (M1_error, M1_error_int, M1_error1, M1_Ks, M1_Kp, M1_Ki, M1_Kd, M1_friction); + M1 = dr_control (M1_pos, M1_set); + pc.printf("M1_power: %f M1_ Direction: ", E1, M1); + // control Motor 2 + E2 = Pw_control (M2_error, M2_error_int, M2_error1, M2_Ks, M2_Kp, M2_Ki, M2_Kd, M2_friction); + M2 = dr_control (M2_pos, M2_set); + pc.printf("M2_power: %f M2_ Direction:", E2, M2); + pc.printf("M1_error: %f M2_ error: \n", M1_error, M2_error); control_go = false; } if (send_go == true) { @@ -317,12 +173,6 @@ send_go = false; } - if (button_Servo == false) { - pc.printf("servo\n"); - Servo(); - } - //-----------End of scedule progrmas---------------- - } -} +} \ No newline at end of file