Kim Bruil
robot positie error test ding
Dependencies: MODSERIAL mbed EMG_Input QEI biquadFilter
Diff: main.cpp
- Revision:
- 9:19dafcb4a098
- Parent:
- 8:681151d89a75
- Child:
- 10:27fdd43f3b85
--- a/main.cpp Wed Oct 26 12:04:11 2016 +0000
+++ b/main.cpp Thu Oct 27 19:36:12 2016 +0000
@@ -57,6 +57,11 @@
double motor2pos=0;
double motor2int=0;
double motor2olderr=0;
+/*
+** Gripper variable
+*/
+PwmOut gripperpwm(D3);
+bool gripperclosed=true;
BiQuadChain motor1bqc_derivative;
BiQuadChain motor2bqc_derivative;
@@ -242,6 +247,24 @@
// robot_init
// Initialise robot
void robot_init() {
+ // Set pwm motor periods
+ gripperpwm.period_ms(20);
+ motor1pwm.period_ms(2);
+ motor2pwm.period_ms(2);
+
+ // Initialise encoders
+ motor1sense.reset();
+ motor2sense.reset();
+
+
+ // Set motor PID D-filters
+ motor1bqc_derivative.add(&bq1).add(&bq2);
+ motor2bqc_derivative.add(&bq3).add(&bq4);
+
+
+ pc.baud(115200); // Set serial communication speed
+
+ // Initialise robot segments
state = R_HORIZONTAL;
// Init arm1 (upper arm) first link
arm1.length = 28.0f;
@@ -259,110 +282,69 @@
end.length = 5.0f;
end.x = 0;
end.y = arm1.length+arm2.length;
- end.theta = 0;
-
- motor1sense.reset();
- motor2sense.reset();
+ end.theta = 0;
}
-int lr_state=0;
-int ud_state=0;
-int sw2_state=0;
float ax=0.01;
float ay=0.01;
-
-float spd=1.0;
+const double maxspeed=3.0f;
+bool moveleft; // Flag set to true if moving to the left
+bool movedown; // Flag set to true if moving to the left
-void inputswitches()
-{
-/* if(switch1.read())
- {
- if (lr_state==0){
- vx=0;
- ledr.write(1);
- }
- }
- else
- {
- lr_state=0;
- vx = vx - ax;
- if (vx<-3.0f){
- vx=-3.0f;
- }
- ledr.write(0);
- }
-
- if(switch2.read())
- {
- if(lr_state==1){
- vx=0;
- ledr.write(1);
- }
- }
- else
- {
- lr_state=1;
- vx = vx + ax;
- if (vx>3.0f){
- vx=3.0f;
- }
- ledr.write(0);
+void r_moveHorizontal(){
+ if (flag_switch){
+ if (!switch1.read()){
+ moveleft = true;
+ vx = (vx<-maxspeed)?-maxspeed:vx-ax;
}
- if(switch3.read())
- {
- if (ud_state==0){
- vy=0;
- ledr.write(1);
- }
- }
- else
- {
- ud_state=0;
- vy = vy - ay;
- if (vy<-3.0f){
- vy=-3.0f;
- }
- ledr.write(0);
- }
-
- if(switch4.read())
- {
- if(ud_state==1){
- vy=0;
- ledr.write(1);
- }
+ else {
+ vx = moveleft?0:vx;
}
- else
- {
- ud_state=1;
- vy = vy + ay;
- if (vy>3.0f){
- vy=3.0f;
- }
- ledr.write(0);
+ if (!switch2.read()){
+ moveleft = false;
+ vx = (vx>maxspeed)?maxspeed:vx+ax;
}
- */
- /*
- if (switch1.read()){
- end.x += spd;
- }
- if (switch2.read()){
- end.x -= spd;
+ else {
+ vx = !moveleft?0:vx;
}
- if (switch3.read()){
- end.y += spd;
- }
- if (switch4.read()){
- end.y -= spd;
- }
- */
+ robot_setSetpoint(end.x+vx, end.y);
+ }
}
-const double maxspeed=3.0f;
+void r_moveVertical(){
+ if (flag_switch){
+ if (!switch1.read()){
+ movedown = true;
+ vy = (vy<-maxspeed)?-maxspeed:vy-ay;
+ }
+ else {
+ vy = movedown?0:vy;
+ }
+ if (!switch2.read()){
+ movedown = false;
+ vy = (vy>maxspeed)?maxspeed:vy+ay;
+ }
+ else {
+ vy = !movedown?0:vy;
+ }
+ robot_setSetpoint(end.x, end.y+vy);
+ }
+}
-void r_moveHorizontal (){
- if (flag_switch){ // Read switches and set setpoint once per switch ticker milliseconds
- flag_switch = false;
+void r_moveGripper(){
+ if(flag_switch){
+ if(!switch1.read() && !gripperclosed){
+ gripperpwm.pulsewidth_us(1035); // Close gripper
+ gripperclosed = true;
+ } else if(!switch2.read() && gripperclosed){
+ gripperpwm.pulsewidth_us(1500); // Open gripper
+ gripperclosed = false;
+ }
+ }
+}
+
+void r_processStateSwitch(){
+ if(flag_switch){
if(switch3.read()){
switch3down = false;
}
@@ -374,126 +356,33 @@
state = R_VERTICAL;
break;
case R_VERTICAL:
+ state = R_GRIPPER;
+ break;
+ case R_GRIPPER:
state = R_HORIZONTAL;
break;
+
}
}
}
- if(switch1.read())
- {
- if (lr_state==0){
- vx=0;
- ledr.write(1);
- }
- }
- else
- {
- lr_state=0;
- motor1int = 0;
- motor2int = 0;
+ }
+}
- vx = vx - ax;
- if (vx<-maxspeed){
- vx=-maxspeed;
- }
- ledr.write(0);
- }
-
- if(switch2.read())
- {
- if(lr_state==1){
- vx=0;
-
- ledr.write(1);
- }
- }
- else
- {
- lr_state=1;
- motor1int = 0;
- motor2int = 0;
- vx = vx + ax;
- if (vx>maxspeed){
- vx=maxspeed;
- }
- ledr.write(0);
- }
- robot_setSetpoint(end.x+vx, end.y);
+void r_switchFlagReset(){
+ if (flag_switch){
+ flag_switch = false;
}
}
-void r_moveVertical (){
- if (flag_switch){ // Read switches and set setpoint once per switch ticker milliseconds
- flag_switch = false;
- if(switch3.read()){
- switch3down = false;
- }
- else {
- if (switch3down==false){
- switch3down = true;
- switch(state){
- case R_HORIZONTAL:
- state = R_VERTICAL;
- break;
- case R_VERTICAL:
- state = R_HORIZONTAL;
- break;
- }
- }
- }
-
- vx = 0;
- if(switch1.read())
- {
- if (ud_state==0){
- vy=0;
- ledr.write(1);
- }
- }
- else
- {
- ud_state=0;
- motor1int = 0;
- motor2int = 0;
-
- vy = vy - ay;
- if (vy<-maxspeed){
- vy=-maxspeed;
- }
- ledr.write(0);
- }
-
- if(switch2.read())
- {
- if(ud_state==1){
- vy=0;
-
- ledr.write(1);
- }
- }
- else
- {
- ud_state=1;
- motor1int = 0;
- motor2int = 0;
- vy = vy + ay;
- if (vy>maxspeed){
- vy=maxspeed;
- }
- ledr.write(0);
- }
- robot_setSetpoint(end.x, end.y+vy);
- }
-}
void setmotor1(float val){
- val = -val;
+ val = -val; // Inverse value for motor 1 because it rotates the other way
motor1dir.write(val>=0?1:0);
- motor1pwm.write(fabs(val)>1?1.0f:pow((double)fabs(val), 0.75));//pow((float)fabs(val),(float)3/4.));
+ motor1pwm.write(fabs(val)>1?1.0f:pow((double)fabs(val), 0.75)); // Use a root to make the motor give more power in low end.
}
void setmotor2(float val){
motor2dir.write(val>=0?1:0);
- motor2pwm.write(fabs(val)>1?1.0f:pow((double)fabs(val), 0.75));
+ motor2pwm.write(fabs(val)>1?1.0f:pow((double)fabs(val), 0.75)); // Use a root to make the motor give more power in low end.
}
@@ -508,23 +397,18 @@
}
void r_doPID(){
- double dt = 1/50.0;
+ double dt = 1/50.0; // PID time step
double m1;
double m2;
- if (flag_PID){
+ if (flag_PID){ // Check PID ticker flag
flag_PID = false;
- motor1pos = -(double)motor1sense.getPulses()*(2*M_PI/8400.0);
- motor2pos = (double)motor2sense.getPulses()*(2*M_PI/8400.0);
- m1 = PID_control(motor1olderr, motor1pos, arm1.theta, motor1int, motor1bqc_derivative, dt, 1, 0.2, 0.1);
- m2 = PID_control(motor2olderr, motor2pos, arm2.theta, motor2int, motor2bqc_derivative, dt, 1, 0.2, 0.1);
-
- setmotor1(m1);
- setmotor2(m2);
-/* printf("e1: %f, e2: %f\n\r", motor1olderr, motor2olderr);
- printf("p1: %f, p2: %f\n\r", motor1pos, motor2pos);
- printf("m1: %f, m2: %f\n\r", m1, m2);
-*/
+ motor1pos = -(double)motor1sense.getPulses()*(2*M_PI/8400.0); // Get motor 1 position. Negative, since motor spins opposite
+ motor2pos = (double)motor2sense.getPulses()*(2*M_PI/8400.0); // Get motor 2 position.
+ m1 = PID_control(motor1olderr, motor1pos, arm1.theta, motor1int, motor1bqc_derivative, dt, 1, 0.2, 0.1); // PID motor 1
+ m2 = PID_control(motor2olderr, motor2pos, arm2.theta, motor2int, motor2bqc_derivative, dt, 1, 0.2, 0.1); // PID motor 2
+ setmotor1(m1); // Set motor 1 speed
+ setmotor2(m2); // Set motor 2 speed
}
}
@@ -544,6 +428,13 @@
}
}
+/*
+** maintickerfunction
+**
+** Function: ticker call back. Sets flags at appropriate timer intervals for
+** reading switches, calculating PID, outputting information via serial and
+** reading EMG signals.
+*/
void maintickerfunction (){
static int cnt=0;
if (cnt%20 == 0){ // 50 times per second
@@ -560,10 +451,9 @@
int main(){
- motor1bqc_derivative.add(&bq1).add(&bq2);
- motor2bqc_derivative.add(&bq3).add(&bq4);
+ // Initialise main ticker
mainticker.attach(&maintickerfunction,0.001f);
- pc.baud(115200);
+
robot_init();
while(true){
switch(state){
@@ -575,7 +465,12 @@
case R_VERTICAL:
r_moveVertical();
break;
+ case R_GRIPPER:
+ r_moveGripper();
+ break;
}
+ r_processStateSwitch();
+ r_switchFlagReset();
r_doPID();
r_outputToMatlab();
}