Biorobotics
acos faalt
Dependencies: HIDScope MODSERIAL QEI mbed
Diff: main.cpp
- Revision:
- 6:05b6b70618db
- Parent:
- 5:ec6dd614aa7e
--- 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