acos faalt
Dependencies: HIDScope MODSERIAL QEI mbed
Diff: main.cpp
- Revision:
- 1:4465c9e203ce
- Parent:
- 0:76bc19ed12ee
- Child:
- 2:da3b7dd2beb0
diff -r 76bc19ed12ee -r 4465c9e203ce main.cpp --- a/main.cpp Tue Oct 06 16:34:46 2015 +0000 +++ b/main.cpp Thu Oct 15 10:42:30 2015 +0000 @@ -4,14 +4,20 @@ #include "QEI.h" //---------- Change motor control parameters------- -float Ks = 0.5; //minimum power required to move -float Kp = 0.65; //strentgh of proportional control -float Kd = 0.12; //strentgh of differential control -float Ki = 0.5; //strentgh of integrational control +//Motor 1 +const float M1_Ks = 0.4; //minimum power required to move +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 +//Motor 2 +const float M2_Ks = 0.4; //minimum power required to move +const float M2_Kp = 0.1; //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 //----------End of control parameters - - //encoder QEI Motor1 (D13, D12, NC, 32); QEI Motor2 (D11, D10, NC, 32); @@ -29,40 +35,40 @@ HIDScope scope(2); //Define Variables -double Deg1; -double Deg2; -double M1error; -double M2error; -double M1output; -double M2output; -double pos1; -double pos2; -double output; -double pref1,pref2; -bool dir1; -bool dir2; -double long gearbox=0.08587786259541984732824427480916; +double M1_dpos, M1_pos; +double M2_dpos, M2_pos; +double M_error, M1_error, M2_error; +double position; +double pref,pref2; +bool M1_dir, M2_dir; +const double long gearbox=0.08587786259541984732824427480916; +double M_error1 = 0; //Timers and Tickers Timer t; Ticker t1,t2,t3; //booleans run program -volatile bool send_go = false, direction_go = false, speed_go = false; +volatile bool send_go = false, setpoint_go = false, speed_go = false, control_go= false; //------------------Activate programs----------- //Activate send data pc -void Send_True() +void D_Send_True() { send_go = true; } // Activate desired location -void M_position_True() +void M_Setpoint_True() { - direction_go = true; + setpoint_go = true; +} +// Controll if motor should go or not +void M_Control_True() +{ + control_go = true; } //read position of motor and drive motor to set position. -void M_speed_True() +void M_Speed_True() { speed_go = true; } @@ -77,53 +83,64 @@ scope.set(0,Motor1.getPulses()); scope.set(1,Motor2.getPulses()); scope.send(); - pc.printf("Deg M1: %f M2: %f\n", Deg1, Deg2); +// pc.printf("Deg M1: %f M2: %f\n", M1_pos, M2_pos); } //Desired Position Motors --> future script emg -void direction() +void Setpoint() +{ + M1_dpos=pot1.read()*360-180; + M2_dpos=pot2.read()*360-180; + pc.printf(" Desired: M1_dpos: %f M1_dir: %i M2_dpos: %f M2_dir: %i \n",M1_dpos, M1_dir, M2_dpos, M2_dir); +} +double Pw_control (double Pulse,double setpoint,double Ks, double Kp,double Ki,double Kd, double friction) { - pos1=pot1.read()*360-180; - pos2=pot2.read()*360-180; - pc.printf(" Desired: pos1: %f dir1: %i pos2: %f dir2: %i \n",pos1, dir1, pos2, dir2); + //read position motor + position = gearbox*Pulse; + // Motor Power + M_error = abs(setpoint-position); + double M_error_Int= M_error_Int+M_error/1e4; + double Ps = Ks; + double Pp = Kp * M_error; //Proportional control + double Pi = Ki * M_error_Int*M_error; //check dit --> nog niet goed //int controll + double Pd = Kd * (M_error-M_error1); //Differtial control + M_error1 = M_error; + double Power = Ps + Pp + Pi + Pd; + pc.printf("Power bf: %f Pi: %f \n", Power, Pi); + + // overcome minimum power required to turn and stop the motor from 'piepen'. + if (Power<friction) { + Power=0; + } else {} + + //print error +// pc.printf("Error: %f Direction: %f Position M: %f Setpoint %f \n", M_error, position, setpoint); + + pc.printf("Power: %f\n", Power); + return Power ;//is it possible to return 2 things for +} +// direction controll +bool Dr_control (double Pulse,double setpoint) +{ + //read position motor + double position = gearbox*Pulse; + // Direction motor should turn + int Direction = (position > setpoint) ? false:true; + return Direction; +} +// begin to set point +void calibration() +{ + M1_dpos=0; + M2_dpos=0; + if (button==true) { + M1_pos=0; + M2_pos=0; + } else { + } } -//translate desired position to movement motor -void speed() -{ -//Read position --> Turn pulses to Degrees - Deg1=gearbox*Motor1.getPulses(); - Deg2=gearbox*Motor2.getPulses(); -//Direction the motors should turn - dir1=(Deg1 > pos1) ? false: true; - dir2=(Deg2 > pos1) ? false: true; - //Motor power - M1error=Ks+Kp*abs((Deg1-pos1)/pos1)+Kd*abs((pref1-abs((Deg1-pos1))/pos1))+Ki*0; - M2error=Ks+Kp*abs((Deg2-pos1)/pos1)+Kd*0+Ki*0; - pref1=abs((Deg1-pos1));//prefious error - pref2=abs((Deg2-pos2));//prefious error - pc.printf("M1error: %f\n", M1error); -//Motor controll - //Motor 1 - if ((Deg1< pos1) || (Deg1> pos1)) { - E1=M1error; - M1=dir1; - pc.printf("M1>P1\n"); - } else { - E1=0; - pc.printf("I am in Else\n"); - } - //Motor 2 - if ((Deg2< pos2) || (Deg2> pos2)) { - E2=M2error; - M2=dir2; - pc.printf("M2>P2\n"); - } else { - E1=0; - pc.printf("I am in Else\n"); - } -} //--------------- End of control programs---------- int main() { @@ -138,25 +155,35 @@ //sub programs - time how fast everything occurs - t1.attach_us(&Send_True, 1e4); //Send data to pc - t2.attach_us(&M_position_True, 1e4); //Desired position motor(EMG goes here) - t3.attach_us(&M_speed_True, 1e4); //Speed control here + t1.attach_us(&D_Send_True, 1e4); //Send data to pc + t2.attach_us(&M_Setpoint_True, 1e4); //Desired position motor(EMG goes here) + t3.attach_us(M_Control_True, 1e4); //Speed control here //-------------Scedule programs------------------- while(1) { - if (send_go==true) { + if (setpoint_go == true) { + Setpoint(); + + setpoint_go = false; + } else if (control_go == true) { + // control Motor1 + E1 = Pw_control (Motor1.getPulses(), M1_dpos, M1_Ks, M1_Kp, M1_Ki, M1_Kd, M1_friction); + M1 = Dr_control (Motor1.getPulses(), M1_dpos); + M1_pos = position; + M1_error = M_error; + // control Motor2 + E2 = Pw_control (Motor2.getPulses(), M2_dpos, M2_Ks, M2_Kp, M2_Ki, M2_Kd, M2_friction); + //M2 = Dr_control (Motor2.getPulses(),M2_dpos); + M2_pos = position; + M2_error = M_error; + //print error + pc.printf("M1_pos = %f M1_error = %f M2_pos = %f M2_error = %f\n", M1_pos, M1_error, M2_pos, M2_error); // once this works place it in 'send' + control_go = false; + } else if (send_go==true) { Send(); send_go = false; - } else if (direction_go == true) { - direction(); - - direction_go = false; - } else if (speed_go == true) { - speed(); - - speed_go = false; } else { //-----------End of scedule progrmas----------------