acos faalt
Dependencies: HIDScope MODSERIAL QEI mbed
main.cpp
- Committer:
- davevogel0
- Date:
- 2015-10-21
- Revision:
- 6:05b6b70618db
- Parent:
- 5:ec6dd614aa7e
File content as of revision 6:05b6b70618db:
#include "mbed.h" #include "HIDScope.h" #include "MODSERIAL.h" #include "QEI.h" //---------- Change motor control parameters------- //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.4; //minimum power required to make the motor move //Motor 2 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 QEI Motor1 (D13, D12, NC, 32); QEI Motor2 (D11, D10, NC, 32); //Define pins DigitalOut M2(D7); //direction 2 //1 is ccw 0=cw PwmOut E2(D6); //speed 2 DigitalOut M1(D4); //direction 1 //1 is ccw 0=cw PwmOut E1(D5); //speed 1 AnalogIn pot1(A0); //read value of pot1 for position AnalogIn pot2(A1); //read value of pot2 for position DigitalOut myled(LED_GREEN); MODSERIAL pc(USBTX, USBRX); DigitalIn button(PTA4); HIDScope scope(2); //Define Variables 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; //Timers and Tickers Timer t; Ticker t1,t2,t3; //booleans run program volatile bool send_go = false, setpoint_go = false, control_go= false; //------------------Activate programs----------- //Activate send data pc void Send_True() { send_go = true; } // Activate desired location void M_Setpoint_True() { setpoint_go = true; } // Controll if motor should go or not void M_Control_True() { control_go = true; } //------------------End of activate programs-------- //------------------Start of control programs------- //Send values over HIDScope & Serial port void Send() { scope.set(0,Motor1.getPulses()); scope.set(1,Motor2.getPulses()); scope.send(); } //Desired Position Motors void M_Setpoint() { M1_set=pot1.read()*360-180; M2_set=pot2.read()*360-180; } // read position of motors void M_pos () { M1_pos = gearbox * Motor1.getPulses(); M2_pos = gearbox * Motor1.getPulses(); } // calculate error between pos and setpoint void M_error () { M1_error= M1_set - M1_pos; M2_error= M2_set - M2_pos; } //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 S_error_int = S_error_int + S_error / 1e4; double Ps = Ks; 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); // overcome minimum power required to turn and stop the motor from 'piepen' Also limit power to a max. if (Power<friction) { Power=0; } else if (Power>0.7) { Power=0.7; } else {} return Power ; } // direction control bool dr_control (double A, double B) { int Direction = (A > B) ? false:true; return Direction; } //--------------- End of control programs---------- int main() { //turn that led off!It hurts my eyes! Ow, I do boot. myled = 1; //PWM period motors E1.period(0.0001f); E2.period(0.0001f); pc.baud(115200); //sub programs - time how fast everything occurs 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(setpoint_go == true) { M_Setpoint(); pc.printf("setpoint\n"); setpoint_go = false; } if (control_go == true) { 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) { pc.printf("send\n"); Send(); send_go = false; } //-----------End of scedule progrmas---------------- } }