Biorobotics / Mbed 2 deprecated PID_Control

acos faalt

Dependencies:   HIDScope MODSERIAL QEI mbed

main.cpp@5:ec6dd614aa7e, 2015-10-21 (annotated)

Committer:
davevogel0
Date:
Wed Oct 21 13:51:43 2015 +0000
Revision:
5:ec6dd614aa7e
Parent:
4:f0fd4a4ec178
Child:
6:05b6b70618db
In the function lengtharm the acos goes to a NaN which causes instability.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
davevogel0 0:76bc19ed12ee 1 #include "mbed.h"
davevogel0 0:76bc19ed12ee 2 #include "HIDScope.h"
davevogel0 0:76bc19ed12ee 3 #include "MODSERIAL.h"
davevogel0 0:76bc19ed12ee 4 #include "QEI.h"
davevogel0 5:ec6dd614aa7e 5 #include "Servo.h"
davevogel0 0:76bc19ed12ee 6
davevogel0 3:337345f748cf 7 //Aray of locations and Length of parts
davevogel0 5:ec6dd614aa7e 8 float X [10] = {-0.4, -0.3, -0.2, -0.1, 0, 0.1, 0.2, 0.3, 0.4, 0.5};
davevogel0 5:ec6dd614aa7e 9 float Y [10] = {-0.4, -0.3, -0.2, -0.1, 0, 0.1, 0.2, 0.3, 0.4, 0.5};
davevogel0 3:337345f748cf 10 const double L1 = 0.34;
davevogel0 3:337345f748cf 11 const double L2 = 0.26;
davevogel0 4:f0fd4a4ec178 12 const double L_board= 0.42;
davevogel0 3:337345f748cf 13 const double R_Max_error = 0.01;
davevogel0 2:da3b7dd2beb0 14
davevogel0 0:76bc19ed12ee 15 //---------- Change motor control parameters-------
davevogel0 1:4465c9e203ce 16 //Motor 1
davevogel0 1:4465c9e203ce 17 const float M1_Ks = 0.4; //minimum power required to move
davevogel0 1:4465c9e203ce 18 const float M1_Kp = 0.1; //strentgh of proportional control
davevogel0 1:4465c9e203ce 19 const float M1_Kd = 0.1; //strentgh of differential control Include timestep in constant
davevogel0 1:4465c9e203ce 20 const float M1_Ki = 0.1; //strentgh of integrational control Include timestep in constant
davevogel0 1:4465c9e203ce 21 const double M1_friction = 0.55; //minimum power required to make the motor move
davevogel0 1:4465c9e203ce 22 //Motor 2
davevogel0 1:4465c9e203ce 23 const float M2_Ks = 0.4; //minimum power required to move
davevogel0 5:ec6dd614aa7e 24 const float M2_Kp = -2; //strentgh of proportional control
davevogel0 1:4465c9e203ce 25 const float M2_Kd = 0.1; //strentgh of differential control Include timestep in constant
davevogel0 1:4465c9e203ce 26 const float M2_Ki = 0.1; //strentgh of integrational control Include timestep in constant
davevogel0 1:4465c9e203ce 27 const double M2_friction = 0.55; //minimum power required to make the motor move
davevogel0 0:76bc19ed12ee 28 //----------End of control parameters
davevogel0 0:76bc19ed12ee 29
davevogel0 0:76bc19ed12ee 30 //encoder
davevogel0 0:76bc19ed12ee 31 QEI Motor1 (D13, D12, NC, 32);
davevogel0 0:76bc19ed12ee 32 QEI Motor2 (D11, D10, NC, 32);
davevogel0 0:76bc19ed12ee 33
davevogel0 0:76bc19ed12ee 34 //Define pins
davevogel0 5:ec6dd614aa7e 35 DigitalOut M2(D7); //direction 2 //1 is ccw 0=cw
davevogel0 5:ec6dd614aa7e 36 PwmOut E2(D6); //speed 2
davevogel0 5:ec6dd614aa7e 37 DigitalOut M1(D4); //direction 1 //1 is ccw 0=cw
davevogel0 5:ec6dd614aa7e 38 PwmOut E1(D5); //speed 1
davevogel0 5:ec6dd614aa7e 39 AnalogIn pot1(A0); //read value of pot1 for position
davevogel0 5:ec6dd614aa7e 40 AnalogIn pot2(A1); //read value of pot2 for position
davevogel0 0:76bc19ed12ee 41 DigitalOut myled(LED_GREEN);
davevogel0 5:ec6dd614aa7e 42 DigitalOut magnet(D8); //magnet is defined on D8
davevogel0 0:76bc19ed12ee 43 MODSERIAL pc(USBTX, USBRX);
davevogel0 0:76bc19ed12ee 44 DigitalIn button(PTA4);
davevogel0 4:f0fd4a4ec178 45 DigitalIn button_Arm1(D1);
davevogel0 4:f0fd4a4ec178 46 DigitalIn button_Arm2(D2);
davevogel0 5:ec6dd614aa7e 47 DigitalIn button_Servo(D0);
davevogel0 0:76bc19ed12ee 48 HIDScope scope(2);
davevogel0 5:ec6dd614aa7e 49 Servo swagvo(D3);
davevogel0 0:76bc19ed12ee 50
davevogel0 0:76bc19ed12ee 51 //Define Variables
davevogel0 1:4465c9e203ce 52 double M1_dpos, M1_pos;
davevogel0 1:4465c9e203ce 53 double M2_dpos, M2_pos;
davevogel0 1:4465c9e203ce 54 double M_error, M1_error, M2_error;
davevogel0 4:f0fd4a4ec178 55 double R_set, A_R, R_error, R_error_int, R_cal, R_cal1, R_cal2;
davevogel0 4:f0fd4a4ec178 56 double Q_set, Q_end, Q_error, Q_error_int, Q_cal, Q_cal1, Q_cal2;
davevogel0 1:4465c9e203ce 57 double position;
davevogel0 3:337345f748cf 58 const double long gearbox = 0.085877862;
davevogel0 2:da3b7dd2beb0 59 const double long PI = 3.14159265359;
davevogel0 1:4465c9e203ce 60 double M_error1 = 0;
davevogel0 4:f0fd4a4ec178 61 int run = 0;
davevogel0 5:ec6dd614aa7e 62 bool H_wheel=0; // 0 is up and 1 is down
davevogel0 5:ec6dd614aa7e 63 bool Magnet=0;
davevogel0 0:76bc19ed12ee 64
davevogel0 0:76bc19ed12ee 65 //Timers and Tickers
davevogel0 0:76bc19ed12ee 66 Timer t;
davevogel0 0:76bc19ed12ee 67 Ticker t1,t2,t3;
davevogel0 0:76bc19ed12ee 68
davevogel0 0:76bc19ed12ee 69 //booleans run program
davevogel0 5:ec6dd614aa7e 70 volatile bool send_go = false, setpoint_go = false, control_go= false, calibration_go= false;
davevogel0 0:76bc19ed12ee 71
davevogel0 0:76bc19ed12ee 72 //------------------Activate programs-----------
davevogel0 0:76bc19ed12ee 73 //Activate send data pc
davevogel0 3:337345f748cf 74 void Send_True()
davevogel0 0:76bc19ed12ee 75 {
davevogel0 0:76bc19ed12ee 76 send_go = true;
davevogel0 0:76bc19ed12ee 77 }
davevogel0 0:76bc19ed12ee 78 // Activate desired location
davevogel0 1:4465c9e203ce 79 void M_Setpoint_True()
davevogel0 0:76bc19ed12ee 80 {
davevogel0 1:4465c9e203ce 81 setpoint_go = true;
davevogel0 1:4465c9e203ce 82 }
davevogel0 1:4465c9e203ce 83 // Controll if motor should go or not
davevogel0 1:4465c9e203ce 84 void M_Control_True()
davevogel0 1:4465c9e203ce 85 {
davevogel0 1:4465c9e203ce 86 control_go = true;
davevogel0 0:76bc19ed12ee 87 }
davevogel0 0:76bc19ed12ee 88 //------------------End of activate programs--------
davevogel0 0:76bc19ed12ee 89
davevogel0 0:76bc19ed12ee 90
davevogel0 0:76bc19ed12ee 91 //------------------Start of control programs-------
davevogel0 0:76bc19ed12ee 92
davevogel0 0:76bc19ed12ee 93 //Send values over HIDScope & Serial port
davevogel0 0:76bc19ed12ee 94 void Send()
davevogel0 0:76bc19ed12ee 95 {
davevogel0 0:76bc19ed12ee 96 scope.set(0,Motor1.getPulses());
davevogel0 0:76bc19ed12ee 97 scope.set(1,Motor2.getPulses());
davevogel0 0:76bc19ed12ee 98 scope.send();
davevogel0 0:76bc19ed12ee 99 }
davevogel0 0:76bc19ed12ee 100
davevogel0 3:337345f748cf 101 //Desired Position Motors
davevogel0 1:4465c9e203ce 102 void Setpoint()
davevogel0 1:4465c9e203ce 103 {
davevogel0 4:f0fd4a4ec178 104 // check if
davevogel0 5:ec6dd614aa7e 105 double R_set1=R_set;
davevogel0 5:ec6dd614aa7e 106 double Q_set1=Q_set;
davevogel0 4:f0fd4a4ec178 107
davevogel0 3:337345f748cf 108 //set desired x and y point in array --> do this with EMG
davevogel0 2:da3b7dd2beb0 109 int dX = pot1.read() * 10;
davevogel0 2:da3b7dd2beb0 110 int dY = pot2.read() * 10;
davevogel0 2:da3b7dd2beb0 111
davevogel0 2:da3b7dd2beb0 112 //Set the length of the arm
davevogel0 3:337345f748cf 113 R_set = sqrt( X[dX] * X[dX] + Y[dY] * Y[dY]); //desired length of the arm
davevogel0 3:337345f748cf 114 Q_set = acos( Y[dY] / X[dX] ); //desired angle of the arm (theta for Qref) [rad]
davevogel0 4:f0fd4a4ec178 115
davevogel0 4:f0fd4a4ec178 116 if (R_set1!=R_set && Q_set1 != Q_set) { // of ipv en
davevogel0 5:ec6dd614aa7e 117 pc.printf("reset\n");
davevogel0 4:f0fd4a4ec178 118 run=run+1;
davevogel0 4:f0fd4a4ec178 119 R_error_int=0;
davevogel0 4:f0fd4a4ec178 120 Q_error_int=0;
davevogel0 5:ec6dd614aa7e 121 }
davevogel0 4:f0fd4a4ec178 122
davevogel0 1:4465c9e203ce 123 }
davevogel0 2:da3b7dd2beb0 124
davevogel0 3:337345f748cf 125 void Lengtharm()
davevogel0 2:da3b7dd2beb0 126 {
davevogel0 2:da3b7dd2beb0 127 //read out length of arm
davevogel0 3:337345f748cf 128 double A_Rx = L1 * cos(gearbox * Motor1.getPulses() * PI / 180.0) + L2 * cos( (gearbox * Motor1.getPulses() + gearbox * Motor2.getPulses())* PI / 180.0);
davevogel0 3:337345f748cf 129 double A_Ry = L1 * sin(gearbox * Motor1.getPulses() * PI / 180.0) + L2 * sin( (gearbox * Motor1.getPulses() + gearbox * Motor2.getPulses())* PI / 180.0);
davevogel0 3:337345f748cf 130 A_R = sqrt( pow (A_Rx, 2) + pow (A_Ry, 2)); //length Arm
davevogel0 3:337345f748cf 131 R_error= A_R-R_set; //error in arm length
davevogel0 2:da3b7dd2beb0 132 }
davevogel0 2:da3b7dd2beb0 133
davevogel0 3:337345f748cf 134 void Qpos()
davevogel0 0:76bc19ed12ee 135 {
davevogel0 3:337345f748cf 136 //read out pos of Motor 1
davevogel0 3:337345f748cf 137 double A_Rx = L1 * cos(gearbox * Motor1.getPulses() * PI / 180.0) + L2 * cos( (gearbox * Motor1.getPulses() + gearbox * Motor2.getPulses())* PI / 180.0);
davevogel0 3:337345f748cf 138 double A_Ry = L1 * sin(gearbox * Motor1.getPulses() * PI / 180.0) + L2 * sin( (gearbox * Motor1.getPulses() + gearbox * Motor2.getPulses())* PI / 180.0);
davevogel0 5:ec6dd614aa7e 139 pc.printf("A_Rx = %f A_Ry = %f", A_Rx, A_Ry);
davevogel0 3:337345f748cf 140 Q_end= acos( A_Ry / A_Rx ); //actual angle of the arm //Aandacht is dit graden(moet in radians)?!!!!!
davevogel0 5:ec6dd614aa7e 141 pc.printf("Q_set = %f Q_end= %f \n", Q_set, Q_end);
davevogel0 3:337345f748cf 142 Q_error= Q_end-Q_set; //error in angle of Qref
davevogel0 3:337345f748cf 143 }
davevogel0 3:337345f748cf 144
davevogel0 3:337345f748cf 145 double Pw_control (double S_error, double& S_error_int, double Ks, double Kp,double Ki,double Kd, double friction)
davevogel0 3:337345f748cf 146 {
davevogel0 1:4465c9e203ce 147 // Motor Power
davevogel0 3:337345f748cf 148 M_error = S_error; //give the amount of error
davevogel0 3:337345f748cf 149
davevogel0 3:337345f748cf 150 double M_error_int = S_error_int + M_error / 1e4;
davevogel0 3:337345f748cf 151 S_error_int = M_error_int;
davevogel0 1:4465c9e203ce 152 double Ps = Ks;
davevogel0 1:4465c9e203ce 153 double Pp = Kp * M_error; //Proportional control
davevogel0 3:337345f748cf 154 double Pi = Ki * M_error_int; //check dit --> nog niet goed //int controll
davevogel0 3:337345f748cf 155 double Pd = Kd * (M_error - M_error1); //Differtial control
davevogel0 1:4465c9e203ce 156 M_error1 = M_error;
davevogel0 1:4465c9e203ce 157 double Power = Ps + Pp + Pi + Pd;
davevogel0 1:4465c9e203ce 158 pc.printf("Power bf: %f Pi: %f \n", Power, Pi);
davevogel0 1:4465c9e203ce 159
davevogel0 3:337345f748cf 160 // overcome minimum power required to turn and stop the motor from 'piepen' Also limit power to a max.
davevogel0 1:4465c9e203ce 161 if (Power<friction) {
davevogel0 1:4465c9e203ce 162 Power=0;
davevogel0 3:337345f748cf 163 } else if (Power>0.7) {
davevogel0 3:337345f748cf 164 Power=0.7;
davevogel0 1:4465c9e203ce 165 } else {}
davevogel0 1:4465c9e203ce 166
davevogel0 1:4465c9e203ce 167 return Power ;//is it possible to return 2 things for
davevogel0 1:4465c9e203ce 168 }
davevogel0 3:337345f748cf 169
davevogel0 1:4465c9e203ce 170 // direction controll
davevogel0 3:337345f748cf 171 bool dr_control (double A, double B)
davevogel0 1:4465c9e203ce 172 {
davevogel0 1:4465c9e203ce 173 // Direction motor should turn
davevogel0 5:ec6dd614aa7e 174 int Direction = (A > B) ? true:false;
davevogel0 1:4465c9e203ce 175 return Direction;
davevogel0 1:4465c9e203ce 176 }
davevogel0 3:337345f748cf 177
davevogel0 5:ec6dd614aa7e 178 void Servo()
davevogel0 5:ec6dd614aa7e 179 {
davevogel0 5:ec6dd614aa7e 180 swagvo.SetPosition(650); //lower wire
davevogel0 5:ec6dd614aa7e 181 wait(1);
davevogel0 5:ec6dd614aa7e 182 magnet = !magnet; //attach piece
davevogel0 5:ec6dd614aa7e 183 wait(1);
davevogel0 5:ec6dd614aa7e 184 swagvo.SetPosition(2350); //raise wire
davevogel0 5:ec6dd614aa7e 185 }
davevogel0 5:ec6dd614aa7e 186
davevogel0 0:76bc19ed12ee 187 //--------------- End of control programs----------
davevogel0 0:76bc19ed12ee 188 int main()
davevogel0 0:76bc19ed12ee 189 {
davevogel0 0:76bc19ed12ee 190 //turn that led off!It hurts my eyes! Ow, I do boot.
davevogel0 2:da3b7dd2beb0 191 myled = 1;
davevogel0 0:76bc19ed12ee 192
davevogel0 0:76bc19ed12ee 193 //PWM period motors
davevogel0 0:76bc19ed12ee 194 E1.period(0.0001f);
davevogel0 0:76bc19ed12ee 195 E2.period(0.0001f);
davevogel0 5:ec6dd614aa7e 196 swagvo.Enable(1500,20000);
davevogel0 0:76bc19ed12ee 197
davevogel0 0:76bc19ed12ee 198 pc.baud(115200);
davevogel0 0:76bc19ed12ee 199
davevogel0 0:76bc19ed12ee 200 //sub programs - time how fast everything occurs
davevogel0 5:ec6dd614aa7e 201 t1.attach_us(&Send_True, 1e6); //Send data to pc
davevogel0 5:ec6dd614aa7e 202 t2.attach_us(&M_Setpoint_True, 1e6); //Desired position motor(EMG goes here)
davevogel0 5:ec6dd614aa7e 203 t3.attach_us(M_Control_True, 1e6); //Speed control here
davevogel0 0:76bc19ed12ee 204
davevogel0 3:337345f748cf 205 //calibration
davevogel0 5:ec6dd614aa7e 206 // 0 point position
davevogel0 5:ec6dd614aa7e 207 pc.printf("0 cal\n");
davevogel0 5:ec6dd614aa7e 208 bool cal0 = true;
davevogel0 5:ec6dd614aa7e 209 while (cal0 == true) {
davevogel0 5:ec6dd614aa7e 210 if (button == false) {
davevogel0 4:f0fd4a4ec178 211 Qpos();
davevogel0 4:f0fd4a4ec178 212 Lengtharm();
davevogel0 4:f0fd4a4ec178 213 Q_cal=Q_end;
davevogel0 4:f0fd4a4ec178 214 R_cal=A_R;
davevogel0 5:ec6dd614aa7e 215 cal0= false;
davevogel0 5:ec6dd614aa7e 216
davevogel0 5:ec6dd614aa7e 217 //show usefull info on serial
davevogel0 5:ec6dd614aa7e 218 pc.printf("Q_set = %f Q_end= %f Q_error = %f \n", Q_set, Q_end, Q_error);
davevogel0 5:ec6dd614aa7e 219 pc.printf("R_set = %f A_R = %f R_error = %f \n", R_set, A_R, R_error);
davevogel0 4:f0fd4a4ec178 220 }
davevogel0 4:f0fd4a4ec178 221 }
davevogel0 5:ec6dd614aa7e 222
davevogel0 5:ec6dd614aa7e 223
davevogel0 4:f0fd4a4ec178 224 // Board point A
davevogel0 5:ec6dd614aa7e 225 wait (0.5);
davevogel0 5:ec6dd614aa7e 226 pc.printf("A cal\n");
davevogel0 5:ec6dd614aa7e 227 bool calA = true;
davevogel0 5:ec6dd614aa7e 228 while (calA == true) {
davevogel0 5:ec6dd614aa7e 229 if (button == false) {
davevogel0 4:f0fd4a4ec178 230 Qpos();
davevogel0 4:f0fd4a4ec178 231 Lengtharm();
davevogel0 4:f0fd4a4ec178 232 Q_cal1=Q_end;
davevogel0 4:f0fd4a4ec178 233 R_cal1=A_R;
davevogel0 5:ec6dd614aa7e 234 calA = false;
davevogel0 5:ec6dd614aa7e 235
davevogel0 5:ec6dd614aa7e 236 //show usefull info on serial
davevogel0 5:ec6dd614aa7e 237 pc.printf("Q_set = %f Q_end= %f Q_error = %f \n", Q_set, Q_end, Q_error);
davevogel0 5:ec6dd614aa7e 238 pc.printf("R_set = %f A_R = %f R_error = %f \n", R_set, A_R, R_error);
davevogel0 4:f0fd4a4ec178 239 }
davevogel0 4:f0fd4a4ec178 240 }
davevogel0 5:ec6dd614aa7e 241
davevogel0 4:f0fd4a4ec178 242 //Board point B
davevogel0 5:ec6dd614aa7e 243
davevogel0 5:ec6dd614aa7e 244 wait (0.5);
davevogel0 5:ec6dd614aa7e 245 pc.printf("B cal\n");
davevogel0 5:ec6dd614aa7e 246 bool calB = true;
davevogel0 5:ec6dd614aa7e 247 while (calB == true) {
davevogel0 5:ec6dd614aa7e 248 if (button == false) {
davevogel0 4:f0fd4a4ec178 249 Qpos();
davevogel0 4:f0fd4a4ec178 250 Lengtharm();
davevogel0 4:f0fd4a4ec178 251 Q_cal2=Q_end;
davevogel0 4:f0fd4a4ec178 252 R_cal2=A_R;
davevogel0 5:ec6dd614aa7e 253 calB = false;
davevogel0 5:ec6dd614aa7e 254
davevogel0 5:ec6dd614aa7e 255 //show usefull info on serial
davevogel0 5:ec6dd614aa7e 256 pc.printf("Q_set = %f Q_end= %f Q_error = %f \n", Q_set, Q_end, Q_error);
davevogel0 5:ec6dd614aa7e 257 pc.printf("R_set = %f A_R = %f R_error = %f \n", R_set, A_R, R_error);
davevogel0 4:f0fd4a4ec178 258 }
davevogel0 4:f0fd4a4ec178 259 }
davevogel0 5:ec6dd614aa7e 260
davevogel0 5:ec6dd614aa7e 261
davevogel0 3:337345f748cf 262 //-------------Schedule programs-------------------
davevogel0 0:76bc19ed12ee 263 while(1) {
davevogel0 4:f0fd4a4ec178 264 if (calibration_go == true) {
davevogel0 5:ec6dd614aa7e 265 pc.printf("calibration R_set = %f R_cal = %f \n", R_set, R_cal);
davevogel0 4:f0fd4a4ec178 266 //setpoint
davevogel0 5:ec6dd614aa7e 267 R_set = R_cal;
davevogel0 5:ec6dd614aa7e 268 Q_set = Q_cal;
davevogel0 4:f0fd4a4ec178 269 if (button_Arm1 == true && button_Arm2 == true ) {
davevogel0 5:ec6dd614aa7e 270 pc.printf("cal is set\n");
davevogel0 4:f0fd4a4ec178 271 Q_end = Q_cal;
davevogel0 4:f0fd4a4ec178 272 A_R = R_cal;
davevogel0 4:f0fd4a4ec178 273 calibration_go == false;
davevogel0 4:f0fd4a4ec178 274 } else {}
davevogel0 5:ec6dd614aa7e 275 } if(setpoint_go == true && calibration_go == false) {
davevogel0 5:ec6dd614aa7e 276 pc.printf("setpoint\n");
davevogel0 4:f0fd4a4ec178 277 if (run == 2) {
davevogel0 5:ec6dd614aa7e 278 pc.printf("Run = %f\n", run);
davevogel0 4:f0fd4a4ec178 279 calibration_go == true;
davevogel0 4:f0fd4a4ec178 280 run = 0;
davevogel0 4:f0fd4a4ec178 281 } else {
davevogel0 3:337345f748cf 282 Setpoint();
davevogel0 4:f0fd4a4ec178 283 }
davevogel0 1:4465c9e203ce 284 setpoint_go = false;
davevogel0 5:ec6dd614aa7e 285 }
davevogel0 5:ec6dd614aa7e 286 if (control_go == true) {
davevogel0 5:ec6dd614aa7e 287 pc.printf("control\n");
davevogel0 3:337345f748cf 288 Lengtharm();
davevogel0 3:337345f748cf 289 Qpos();
davevogel0 3:337345f748cf 290 if (-1*R_Max_error<R_error<R_Max_error) {
davevogel0 5:ec6dd614aa7e 291 pc.printf("Length control R_error = %f R_set = %f A_R = %f\n", R_error, R_set, A_R);
davevogel0 3:337345f748cf 292 // control Motor2
davevogel0 3:337345f748cf 293 E2 = Pw_control (R_error, R_error_int, M2_Ks, M2_Kp, M2_Ki, M2_Kd, M2_friction);
davevogel0 3:337345f748cf 294 M2 = dr_control (A_R,R_set);
davevogel0 3:337345f748cf 295 M2_error = M_error;
davevogel0 3:337345f748cf 296 //print error
davevogel0 5:ec6dd614aa7e 297 pc.printf("R_error = %f\n", R_error);
davevogel0 5:ec6dd614aa7e 298 } else if (-1*R_Max_error<Q_error<R_Max_error) {
davevogel0 5:ec6dd614aa7e 299 pc.printf("Angle control Q_error = %f Q_set = %f Q_end = %f\n", Q_error, Q_set, Q_end);
davevogel0 3:337345f748cf 300 // control Motor 1
davevogel0 5:ec6dd614aa7e 301 E1 = 0;//Pw_control (Q_error, Q_error_int, M1_Ks, M1_Kp, M1_Ki, M1_Kd, M1_friction);
davevogel0 3:337345f748cf 302 M1 = dr_control (Q_end, Q_set);
davevogel0 3:337345f748cf 303 // control Motor 2 - has to do nothing
davevogel0 3:337345f748cf 304 E2 = 0;
davevogel0 5:ec6dd614aa7e 305 // print error
davevogel0 5:ec6dd614aa7e 306 pc.printf("Q_error = %f\n", Q_error);
davevogel0 5:ec6dd614aa7e 307 } else {
davevogel0 5:ec6dd614aa7e 308 E1 = 0;
davevogel0 5:ec6dd614aa7e 309 E2 = 0;
davevogel0 5:ec6dd614aa7e 310 pc.printf("i dont know what to do R_error = %f R_set = %f A_R = %f\n", R_error, R_set, A_R);
davevogel0 5:ec6dd614aa7e 311 }
davevogel0 1:4465c9e203ce 312 control_go = false;
davevogel0 5:ec6dd614aa7e 313 }
davevogel0 5:ec6dd614aa7e 314 if (send_go == true) {
davevogel0 5:ec6dd614aa7e 315 pc.printf("send\n");
davevogel0 0:76bc19ed12ee 316 Send();
davevogel0 0:76bc19ed12ee 317
davevogel0 0:76bc19ed12ee 318 send_go = false;
davevogel0 5:ec6dd614aa7e 319 }
davevogel0 5:ec6dd614aa7e 320 if (button_Servo == false) {
davevogel0 5:ec6dd614aa7e 321 pc.printf("servo\n");
davevogel0 5:ec6dd614aa7e 322 Servo();
davevogel0 5:ec6dd614aa7e 323 }
davevogel0 0:76bc19ed12ee 324
davevogel0 0:76bc19ed12ee 325 //-----------End of scedule progrmas----------------
davevogel0 5:ec6dd614aa7e 326
davevogel0 0:76bc19ed12ee 327 }
davevogel0 0:76bc19ed12ee 328 }