![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Signa-bot code for project BioRobotics, at University of Twente.
Dependencies: mbed QEI MODSERIAL FastPWM ttmath Math
Motor_tryout.cpp
- Committer:
- viviien
- Date:
- 2019-11-02
- Revision:
- 37:15446e49ff3d
- Parent:
- 35:4cb2ed6dd2d2
File content as of revision 37:15446e49ff3d:
#include "mbed.h" #include "MODSERIAL.h" #include "QEI.h" #include "Math.h" #include "ttmath.h" #include "FastPWM.h" MODSERIAL pc(USBTX, USBRX); //Serial term (USBTX, USBRX); FastPWM motor1_pwm(PTC2); DigitalOut motor1_dir(PTC3); FastPWM motor2_pwm(PTA2); DigitalOut motor2_dir(PTB23); FastPWM motor3_pwm(PTC4); DigitalOut motor3_dir(PTC12); AnalogIn potmeter1(A1); QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING); QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING); QEI Encoder3(D2,D3,NC,64,QEI::X4_ENCODING); int quit; int limit_pos = 8400; float steps; int g = 0; int counts1 = 0; int counts2 = 0; int counts3 = 0; float savedX = 0; float savedY = 0; float savedZ = 0; int sign = 0; const float le = 15.0; const float f = 37.5; const float re = 174.0; const float rf = 50.0; const float pi = 3.14159265358979323846; const float cospi = -0.5; const float sinpi = 0.8660254; float y2; float y1; float z1; float z2; float rje2; float rje; float r2; float r; float z0=-158; float y0=0; float x0=0; float theta1; float theta2; float theta3; // Constant values for PI float Kp; float Ts = 0.0025; float error1 = 0; float error2 = 0; float error3 = 0; float newmotor1 = 1; float newmotor2 = 1; float newmotor3 = 1; float u_k1 = 0; float u_k2 = 0; float u_k3 = 0; float angle1 = 0; float angle2 = 0; float angle3 = 0; float time_sin = 0; void delta_calctheta1 () { float y2 = y0 + le; float y1 = f; float z1 = 0.0; float z2 = z0; float rje2 = re*re - x0*x0; float rje = sqrt(rje2); float r2 = (y1-y2)*(y1-y2) + (z1-z0)*(z1-z0); float r = sqrt(r2); if ((r+rje<rf) || (r + rf <rje) || (rf+rje<r)) { int check = 1; pc.printf("\n\rPunt bestaat niet"); } else { float alpha = acos((r2 + rf*rf -rje2)/(2*rf*r)); float beta = atan((z1+z2)/(y1-y2)); if(beta<=0) { beta = beta + pi; if (beta>=alpha) { theta1 = beta-alpha; } else { theta1 = -alpha+beta; } } if(beta>0) { if (beta<=alpha) { theta1 = -alpha+beta; } else { theta1 = beta-alpha; } } } } void delta_calctheta2 () { float xref = x0*cospi+y0*sinpi; float yref = y0*cospi-x0*sinpi; float zref = z0; float y2 = yref + le; float y1 = f; float z1 = 0.0; float z2 = zref; float rje2 = re*re - xref*xref; float rje = sqrt(rje2); float r2 = (y1-y2)*(y1-y2) + (z1-zref)*(z1-zref); float r = sqrt(r2); if ((r+rje<rf) || (r + rf <rje) || (rf+rje<r)) { int check = 1; pc.printf("\n\rPunt bestaat niet"); } else { float alpha2 = acos((r2 + rf*rf -rje2)/(2*rf*r)); float beta2 = atan((z1+z2)/(y1-y2)); if(beta2<=0) { beta2 = beta2 + pi; if (beta2>=alpha2) { theta2 = beta2-alpha2; } else { theta2 = -alpha2+beta2; } } if(beta2>0) { if (beta2<=alpha2) { theta2 = -alpha2+beta2; } else { theta2 = beta2-alpha2; } } } } void delta_calctheta3 () { float xreff = x0*cospi-y0*sinpi; float yreff = y0*cospi+x0*sinpi; float zreff = z0; float y2 = yreff + le; float y1 = f; float z1 = 0.0; float z2 = zreff; float rje2 = re*re - xreff*xreff; float rje = sqrt(rje2); float r2 = (y1-y2)*(y1-y2) + (z1-zreff)*(z1-zreff); float r = sqrt(r2); if ((r+rje<rf) || (r + rf <rje) || (rf+rje<r)) { int check = 1; pc.printf("\n\rPunt bestaat niet"); } else { float alpha3 = acos((r2 + rf*rf -rje2)/(2*rf*r)); float beta3 = atan((z1+z2)/(y1-y2)); if(beta3<=0) { beta3 = beta3 + pi; if (beta3>=alpha3) { theta3 = beta3-alpha3; } else { theta3 = -alpha3+beta3; } } if(beta3>0) { if (beta3<=alpha3) { theta3 = -alpha3+beta3; } else { theta3 = beta3-alpha3; } } } } Ticker motor_timer; void motor(){ counts1 = Encoder1.getPulses(); counts2 = Encoder2.getPulses(); counts3 = Encoder3.getPulses(); //z0 = 158 - 60* sin(3*time_sin); float r = 20*(1-1/(1+(time_sin))); x0 = r*sin(3*time_sin); y0 = r*cos(3*time_sin); delta_calctheta1 (); delta_calctheta2 (); delta_calctheta3 (); angle1 = counts1/(8400.0)*2.0*pi; angle2 = counts2/(8400.0)*2.0*pi; angle3 = counts3/(8400.0)*2.0*pi; error1 = theta1 - angle1; error2 = theta2 - angle2; error3 = theta3 - angle3; Kp = potmeter1 * 10; u_k1 = 10 * error1; u_k2 = 10 * error2; u_k3 = 10 * error3; newmotor1 = u_k1; newmotor2 = u_k2; newmotor3 = u_k3; if (newmotor1>1.0f){ newmotor1 = 1.0f; } if (newmotor1<-1.0f){ newmotor1 = -1.0f; } if (newmotor2>1.0f){ newmotor2 =1.0f; } if (newmotor2<-1.0f){ newmotor2 = -1.0f; } if (newmotor3>1.0f){ newmotor3 =1.0f; } if (newmotor3<-1.0f){ newmotor3 = -1.0f; } motor1_pwm.write(fabs(newmotor1)); motor1_dir.write(newmotor1<0); motor2_pwm.write(fabs(newmotor2)); motor2_dir.write(newmotor2>0); motor3_pwm.write(fabs(newmotor3)); motor3_dir.write(newmotor3<0); time_sin += 0.002; } /////////////////// // MAIN FUNCTION // /////////////////// int main(void) { delta_calctheta1 (); delta_calctheta2 (); delta_calctheta3 (); pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3); motor_timer.attach(&motor, 0.002); int frequency_pwm = 10000; //10 kHz PWM motor1_pwm.period_us(6); motor2_pwm.period_us(6); motor3_pwm.period_us(6); pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3); pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3); pc.printf("\n\rposition (%f %f %f)", x0, y0, z0); char cc = pc.getc(); pc.baud(115200); while(true){ // while (boxcheckC == 0) { // if (boxcheckA == 1 && x0>=-76 && x0<=75) { // x0=x0+1.0f ; // wait(1.0/20); // } // if (boxcheckB== 1 && x0>=-75 && x0<=76) { // x0=x0-1.0f; // wait(1.0/20); // } // } // // wait(1.5); // savedX = x0; // // while (boxcheckC == 0) { // if (boxcheckA==1 && y0>=-76 && y0<=75){ // y0=y0+1.0f; // wait(1.0/20); // } // if (boxcheckB==1 && y0>=-75 && y0<=76){ // y0=y0-1.0f; // wait(1.0/20); // } // } // // wait(1.5); // savedY = y0; // // while (boxcheckC == 0) { // if (boxcheckA == 1 && z0>=-179 && z0<=-158){ // z0=z0+1.0f; // wait(1.0/20); // } // if (boxcheckB == 1 && z0>=-178 && z0<=-157){ // z0=z0-1.0f; // wait(1.0/20); // } // } // // wait(1.5); // savedZ = z0; // // if (boxcheckC == 1) { // sign = 1; // } // // wait(5.0); // // sign = 0; // // float diffZ = -158 - z0; // float diffX = 0 - x0; // float diffY = 0 - y0; // // for (int i=0; i<=diffZ; i++) { // z0 = z0+1; // } // // for (int i=0; i<=diffY; i++) { // if (diffY>0) { // y0 = y0+1; // } // if (diffY<0) { // y0 = y0-1; // } // } // // for (int i=0; i<=diffX; i++) { // if (diffX>0) { // x0 = x0+1; // } // if (diffX<0) { // x0 = x0-1; // } // } // //break; // pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3); // pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3); // wait(0.1); char cc = pc.getc(); if (cc=='d' && x0>=-76 && x0<=75) { x0=x0+1.0f ; } if (cc=='a' && x0>=-75 && x0<=76) { x0=x0-1.0f; } if (cc=='w' && y0>=-76 && y0<=75){ y0=y0+1.0f; pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3); pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3); pc.printf("\n\rposition (%f %f %f)", x0, y0, z0); } if (cc=='s' && y0>=-75 && y0<=76){ y0=y0-1.0f; pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3); pc .printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3); pc.printf("\n\rposition (%f %f %f)", x0, y0, z0); } if (cc=='u' && z0>=-211 && z0<=-130){ pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3); pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3); pc.printf("\n\rposition (%f %f %f)", x0, y0, z0); z0=z0+1.0f; } if (cc=='j' && z0>=-210 && z0<=-129){ pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3); pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3); pc.printf("\n\rposition (%f %f %f)", x0, y0, z0); z0=z0-1.0f; } } //END MAIN }