Diff: main.cpp
- Revision:
- 21:a316452da8cd
- Parent:
- 20:0f6a88f29a71
- Parent:
- 19:fb3d570a115e
- Child:
- 22:335a30b0825d
--- a/main.cpp Mon Oct 21 12:09:28 2019 +0000
+++ b/main.cpp Mon Oct 21 13:23:11 2019 +0000
@@ -6,6 +6,8 @@
#include "FastPWM.h"
#include <iostream>
MODSERIAL pc(USBTX, USBRX);
+QEI motor2_pos (D8, D9, NC, 32);
+QEI motor1_pos (D12, D13, NC, 32);
AnalogIn ain2(A2);
AnalogIn ain1(A3);
DigitalOut dir2(D4);
@@ -23,7 +25,7 @@
Timeout EMG_peak;
Timeout turn;
Ticker sample_timer;
-HIDScope scope( 2);
+HIDScope scope( 4);
DigitalOut ledred(LED_RED);
DigitalOut ledblue(LED_BLUE);
DigitalOut ledgreen(LED_GREEN);
@@ -40,6 +42,7 @@
volatile bool ignore_turn=true;
enum states{Waiting,Position_calibration,EMG_calibration,Homing,Operating,Demo,Failure};
states currentState=Waiting;
+volatile bool motor1_calibrated=false;
static float v_refx; //reference speeds
static float v_refy;
@@ -48,14 +51,9 @@
volatile float theta1_old = 0.0; //feedback variables
volatile float theta2_old = 0.0;
-int m1_count;
-float m1_angle;
-int m2_count;
-float m2_angle;
-float pi = 3.14;
-float error_x;
-float error_y;
+float error_1;
+float error_2;
//PID controller variables
bool q = true;
@@ -67,10 +65,16 @@
float u_p;
float u_i;
float u_d;
-float error_integral = 0.0;
+float error_integral1 = 0.0;
+float error_integral2 = 0.0;
float u_1;
float u_2;
+const float angle2_offset=asin(0.2);
+const float angle1_offset=asin(3.8/35.0);
+const double pi=3.1415926535897932384626;
+volatile float theta1;
+volatile float theta2;
void read_emg()
{
@@ -112,18 +116,69 @@
RMS1[count1%150]=If1;
/* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
P1=sqrt(RMS_value1);
- scope.set(0, P0 ); // send root mean squared
- scope.set(1, notched0);
/* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
* Ensure that enough channels are available (HIDScope scope( 2 ))
* Finally, send all channels to the PC at once */
- scope.send();
/* To indicate that the function is working, the LED is toggled */
ledred=1;
ledgreen=0;
ledblue=1;
}
+void get_angles(void)
+{
+ float pulses1=motor1_pos.getPulses();
+ float pulses2=motor2_pos.getPulses();
+ theta1=angle1_offset+pulses1*17.0/16.0*2*pi/131.0/32.0;
+ theta2=angle2_offset+pulses2*17.0/16.0*2*pi/131.0/32.0;
+}
+
+void pos_cal(void)
+{
+ float t=state_time.read();
+ static int pos_time_counter=0;
+ static int last_ticks=10000;
+ float pulses;
+ pos_time_counter+=1;
+ if(!motor1_calibrated&&t>1.0f)
+ {
+ dir1=1; //???
+ motor1_pwm.write(0.8f);
+ pulses=motor1_pos.getPulses();
+ if(pos_time_counter%500==0&&fabs(pulses-last_ticks)<1)
+ {
+ ledblue=!ledblue;
+ motor1_pos.reset();
+ last_ticks=10000;
+ state_time.reset();
+ dir1=!dir1;
+ }
+ else if(pos_time_counter%500==0)
+ {
+ last_ticks=motor1_pos.getPulses();
+ }
+
+ }
+ else if(t>1.0f)
+ {
+ motor1_pwm.write(0.0f);
+ dir2=1; //???
+ motor2_pwm.write(0.8f);
+ pulses=motor2_pos.getPulses();
+ if(pos_time_counter%500==0&&fabs(pulses-last_ticks)<1)
+ {
+ ledblue=!ledblue;
+ motor2_pos.reset();
+ motor2_pwm.write(0.0f);
+ }
+ else if(pos_time_counter%500==0)
+ {
+ last_ticks=motor2_pos.getPulses();
+ }
+ }
+
+}
+
void record_min_max(void)
{
float t=state_time.read();
@@ -195,39 +250,71 @@
}
void error(void){
- m1_count = motor1.getPulses(); // Read out both motor counts and convert them to rads.
- m1_angle = (float)m1_count / 4200.0f * (2.0f * pi);
- m2_count = motor2.getPulses();
- m2_angle = (float)m1_count / 4200.0f * (2.0f * pi);
- float dvd=L * (tan(m2_angle) * pow(tan(m1_angle),2) - tan(m1_angle) * pow(tan(m2_angle),2));
+ float dvd=L * (tan(theta2) * pow(tan(theta1),2) - tan(theta1) * pow(tan(theta2),2));
- float a = ( -pow(cos(m1_angle),2) * pow((tan(m1_angle)+tan(m2_angle)),2) * pow(tan(m1_angle),2) ) / dvd; // inverse matrix components of differential x and differential y
- float b = ( pow(cos(m1_angle),2) * pow((tan(m1_angle)+tan(m2_angle)),2) * tan(m1_angle) ) / dvd;
- float c = ( pow(cos(m2_angle),2) * pow((tan(m1_angle)+tan(m2_angle)),2) * pow(tan(m2_angle),2) ) / dvd;
- float d = ( pow(cos(m2_angle),2) * pow((tan(m1_angle)+tan(m2_angle)),2) * pow(tan(m2_angle),2) ) / dvd;
+ float a = ( -pow(cos(theta1),2) * pow((tan(theta1)+tan(theta2)),2) * pow(tan(theta1),2) ) / dvd; // inverse matrix components of differential x and differential y
+ float b = ( pow(cos(theta1),2) * pow((tan(theta1)+tan(theta2)),2) * tan(theta1) ) / dvd;
+ float c = ( pow(cos(theta2),2) * pow((tan(theta1)+tan(theta2)),2) * pow(tan(theta2),2) ) / dvd;
+ float d = ( pow(cos(theta2),2) * pow((tan(theta1)+tan(theta2)),2) * pow(tan(theta2),2) ) / dvd;
float theta1_dot = a*v_refx + b*v_refy;
float theta2_dot = c*v_refx + d*v_refy;
- float theta1_ref = theta1_old+theta1_dot*T;
- float theta2_ref = theta2_old+theta2_dot*T;
-
- error_x = m1_angle - theta1_ref;
- error_y = m2_angle - theta2_ref;
+ float theta1_ref;
+ float theta2_ref;
+ if(currentState==Operating)
+ {
+ theta1_ref = theta1_old+theta1_dot*T;
+ theta2_ref = theta2_old+theta2_dot*T;
+ }
+ else if(currentState==Homing)
+ {
+ theta1_ref=pi/4.0f;
+ theta2_ref=pi/4.0f;
+ }
+ error_1 = theta1 - theta1_ref;
+ error_2 = theta2 - theta2_ref;
theta1_old = theta1_ref;
theta2_old = theta2_ref;
}
-float PID(float err){
+float PID1(float err){
//P action
- u_p = Kp * fabs(err);
+ u_p = Kp * err;
//I action
- error_integral = error_integral + err * T;
- u_i = Ki * error_integral;
+ error_integral1 = error_integral1 + err * T;
+ u_i = Ki * error_integral1;
+
+/*
+ //D action
+ if(q){
+ error_prev = err;
+ q=false;
+ }
+
+ float error_derivative = (err - error_prev)/T;
+ float filtered_error_derivative = LowPassFilter.step(error_derivative);
+ u_d = Kd * filtered_error_derivative;
+ error_prev = err;
+
+*/
+
+ u = u_p + u_i;
+
+ return u;
+ }
+
+float PID2(float err){
+ //P action
+ u_p = Kp * err;
+
+ //I action
+ error_integral2 = error_integral2 + err * T;
+ u_i = Ki * error_integral2;
/*
//D action
@@ -249,20 +336,21 @@
}
void setPWM_controller(void){
- u_1 = PID(error_x);
- u_2 = PID(error_y);
+ error();
+ u_1 = PID1(error_1);
+ u_2 = PID2(error_2);
if(u_1 < 0.0f){
- dir1 = 1;
+ dir1 = 0;
}else{
- dir1 = 0;
+ dir1 = 1;
}
motor1_pwm.write(fabs(u_1));
if(u_2 < 0.0f){
- dir2 = 1;
+ dir2 = 0;
}else{
- dir2 = 0;
+ dir2 = 1;
}
motor2_pwm.write(fabs(u_1));
@@ -270,6 +358,12 @@
void sample()
{
+ get_angles();
+ scope.set(0,P0);
+ scope.set(1,P1);
+ scope.set(2,theta1);
+ scope.set(3,theta2);
+ scope.send();
switch(currentState)
{
case Waiting:
@@ -281,6 +375,7 @@
ledred=1;
ledgreen=1;
ledblue=0;
+ pos_cal();
break;
case EMG_calibration:
ledred=1;
@@ -290,6 +385,7 @@
record_min_max();
break;
case Homing:
+ setPWM_controller();
ledred=0;
ledgreen=1;
ledblue=0;
@@ -314,6 +410,15 @@
motor2_pwm.write(0.0);
break;
}
+ //Preventing the machine from breaking
+ if(theta1>=1.6f)
+ {
+ dir1=1;
+ }
+ if(theta2>=1.6f)
+ {
+ dir2=2;
+ }
}
void error_occur()
@@ -321,10 +426,10 @@
currentState=Failure;
}
-void button_press()
+void button_press(void)
//Press button to change state
{
- state_time.start();
+ state_time.reset();
switch(currentState)
{
case Waiting:
@@ -332,7 +437,15 @@
wait(1);
break;
case Position_calibration:
- currentState=EMG_calibration;
+ if(!motor1_calibrated)
+ {
+ motor1_calibrated=true;
+ wait(1);
+ }
+ else
+ {
+ currentState=EMG_calibration;
+ }
wait(1);
break;
case EMG_calibration:
@@ -365,7 +478,7 @@
int frequency_pwm=10000;
motor1_pwm.period(1.0/frequency_pwm);
motor2_pwm.period(1.0/frequency_pwm);
-
+ state_time.start();
while (true) {
wait(10);
}
