robot positie error test ding
Dependencies: MODSERIAL mbed EMG_Input QEI biquadFilter
Diff: main.cpp
- Revision:
- 9:19dafcb4a098
- Parent:
- 8:681151d89a75
- Child:
- 10:27fdd43f3b85
diff -r 681151d89a75 -r 19dafcb4a098 main.cpp --- 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(); }