Vivien van Veldhuizen
Signa-bot code for project BioRobotics, at University of Twente.
Dependencies: mbed QEI MODSERIAL FastPWM ttmath Math
Diff: Motor_tryout.cpp
- Revision:
- 35:4cb2ed6dd2d2
- Parent:
- 33:88fbf14d8aaf
- Child:
- 36:22d1bcb82061
- Child:
- 37:15446e49ff3d
diff -r 88fbf14d8aaf -r 4cb2ed6dd2d2 Motor_tryout.cpp
--- a/Motor_tryout.cpp Thu Oct 31 15:02:17 2019 +0000
+++ b/Motor_tryout.cpp Fri Nov 01 08:24:42 2019 +0000
@@ -22,22 +22,21 @@
QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
QEI Encoder3(D2,D3,NC,64,QEI::X4_ENCODING);
-float steps1 = 0;
-float steps2 = 0;
-float steps3 = 0;
-int check;
+
int quit;
int limit_pos = 8400;
float steps;
int g = 0;
-bool check1;
-bool check2;
-bool check3;
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;
@@ -55,20 +54,17 @@
float r2;
float r;
-float z0=-172;
+float z0=-158;
float y0=0;
float x0=0;
float theta1;
float theta2;
float theta3;
-float oldtheta1=0;
-float oldtheta2=0;
-float oldtheta3=0;
+
// Constant values for PI
float Kp;
-float Kd;
float Ts = 0.0025;
float error1 = 0;
@@ -83,23 +79,10 @@
float u_k2 = 0;
float u_k3 = 0;
-float u_d1 = 0;
-float u_d2 = 0;
-float u_d3 = 0;
-
-float lasterror1 = 0;
-float lasterror2 = 0;
-float lasterror3 = 0;
-
-float derivative1 = 0;
-float derivative2 = 0;
-float derivative3 = 0;
-
float angle1 = 0;
float angle2 = 0;
float angle3 = 0;
-bool timer = false;
float time_sin = 0;
@@ -119,11 +102,25 @@
}
else {
float alpha = acos((r2 + rf*rf -rje2)/(2*rf*r));
- float beta = atan((z1-z2)/(y1-y2));
- if(beta<0) {
+ float beta = atan((z1+z2)/(y1-y2));
+ if(beta<=0) {
beta = beta + pi;
+ if (beta>=alpha) {
+ theta1 = beta-alpha;
+ }
+ else {
+ theta1 = -alpha+beta;
+ }
+
}
- theta1 = (beta - alpha);
+ if(beta>0) {
+ if (beta<=alpha) {
+ theta1 = -alpha+beta;
+ }
+ else {
+ theta1 = beta-alpha;
+ }
+ }
}
}
@@ -144,19 +141,33 @@
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;
+ 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;
+ }
+
}
- theta2 = (beta - alpha);
+ 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+y0*sinpi;
+ float yreff = y0*cospi+x0*sinpi;
float zreff = z0;
float y2 = yreff + le;
float y1 = f;
@@ -171,13 +182,27 @@
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;
+ 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;
+ }
+
}
- theta3 = (beta - alpha);
+ if(beta3>0) {
+ if (beta3<=alpha3) {
+ theta3 = -alpha3+beta3;
+ }
+ else {
+ theta3 = beta3-alpha3;
+ }
+ }
}
}
@@ -191,11 +216,13 @@
counts2 = Encoder2.getPulses();
counts3 = Encoder3.getPulses();
-// z0 = -172.0 + 10* sin(time_sin);
+//z0 = 158 - 60* sin(3*time_sin);
-float r = 20*(1-1/(1+(time_sin)));
- x0 = r*sin(6.28 * time_sin);
- y0 = r*cos(6.28 * time_sin);
+if (sign == 1) {
+ float r = 20*(1-1/(1+(time_sin)));
+ x0 = savedX0 - r*sin(3*time_sin);
+ y0 = savedY0 - r*cos(3*time_sin);
+}
delta_calctheta1 ();
@@ -210,18 +237,18 @@
error2 = theta2 - angle2;
error3 = theta3 - angle3;
- Kp = potmeter1 * 25;
+ Kp = potmeter1 * 10;
- u_k1 = Kp * error1;
- u_k2 = Kp * error2;
- u_k3 = Kp * error3;
+ 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;
+ newmotor1 = 1.0f;
}
if (newmotor1<-1.0f){
newmotor1 = -1.0f;
@@ -241,6 +268,7 @@
newmotor3 = -1.0f;
}
+
motor1_pwm.write(fabs(newmotor1));
motor1_dir.write(newmotor1<0);
@@ -259,20 +287,157 @@
// MAIN FUNCTION //
///////////////////
-int main(void) {
- motor_timer.attach(&motor, 0.002);
+int main(void) {
+
+ delta_calctheta1 ();
+ delta_calctheta2 ();
+ delta_calctheta3 ();
+
+ pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
- pc.baud(115200);
char cc = pc.getc();
+ 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);
+
+
+
+ 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
}
\ No newline at end of file