control for robotic arm that can play chess using a granular gripper
Dependencies: Encoder mbed HIDScope Servo MODSERIAL
Fork of chessRobot by
Diff: actuators.cpp
- Revision:
- 111:43c0881fe7e7
- Parent:
- 110:a6439e13be8b
- Child:
- 112:7b964afb97b4
--- a/actuators.cpp Tue Oct 27 14:45:15 2015 +0000 +++ b/actuators.cpp Tue Oct 27 17:10:33 2015 +0000 @@ -54,12 +54,13 @@ const int servoStartPos = 1300; // Servo ranges from 600ms(-90) to 2000ms(90), 1300 is 0 deg // Set calibration values -double motorCalSpeed = 10; // deg/sec -double returnSpeed = -10; +double motor1CalSpeed = -5; // deg/sec +double motor2CalSpeed = 5; bool springHit = false; float lastCall = 0; bool calibrating1 = true; bool calibrating2 = false; +double looseTime = 0; // Create object instances // Safety Pin @@ -115,13 +116,11 @@ void motorControl(){ - #ifndef SETPOS // if not tuning with potmeters, switch to EMG // EMG signals to motor speeds // const double scaleVel = 20; // motor1SetSpeed = x_velocity*scaleVel; // motor2SetSpeed = y_velocity*scaleVel; // servoSpeed = z_velocity*scaleVel; - #endif // get encoder positions in degrees // 131.25:1 gear ratio // getPosition uses X2 configuration, so 32 counts per revolution @@ -131,15 +130,15 @@ encoder2Counts = encoder2.getPosition(); - motor1Pos = -((encoder1Counts/32)/131.25)*360; - motor2Pos = -((encoder2Counts/32)/131.25)*360; + motor1Pos = ((encoder1Counts/32)/131.25)*360; + motor2Pos = ((encoder2Counts/32)/131.25)*360; // check if motor's are within rotational boundarys // get encoder speeds in deg/sec now = t.read(); timechange = (now - prevTime); - motor1Speed = -((((encoder1Counts - motor1PrevCounts)/32)/131.25)*360)/timechange; - motor2Speed = -((((encoder2Counts - motor2PrevCounts)/32)/131.25)*360)/timechange; + motor1Speed = ((((encoder1Counts - motor1PrevCounts)/32)/131.25)*360)/timechange; + motor2Speed = ((((encoder2Counts - motor2PrevCounts)/32)/131.25)*360)/timechange; prevTime = now; motor1PrevCounts = encoder1Counts; motor2PrevCounts = encoder2Counts; @@ -163,17 +162,17 @@ motor2PID.Compute(); // write new values to motor's if (motor1SetSpeed > 0 ){ // CCW rotation - direction1 = false; + direction1 = true; motor1PID.SetOutputLimits(0,1); // change pid output direction }else{ - direction1 = true; // CW rotation + direction1 = false; // CW rotation motor1PID.SetOutputLimits(-1,0); } if (motor2SetSpeed > 0 ){ // CCW rotation - direction2 = false; + direction2 = true; motor2PID.SetOutputLimits(0,1); }else{ - direction2 = true; // CW rotation + direction2 = false; // CW rotation motor2PID.SetOutputLimits(-1,0); } motor1Dir.write(direction1); @@ -190,46 +189,52 @@ bool calibrateMotors(){ safetyOn = false; // safety springs off motorsEnable = true; // motors on - redLed.write(0); greenLed.write(1); blueLed.write(1); while (calibrating1 || calibrating2){ if (calibrating1){ - motor1SetSpeed = motorCalSpeed; + motor1SetSpeed = motor1CalSpeed; redLed.write(1); greenLed.write(0); blueLed.write(1); - if(safetyIn.read() !=1){ // check if arm reached safety position + if(safetyIn.read() !=1 && !springHit){ // check if arm reached safety position encoder1.setPosition(0); // set motor 1 cal angle - motor1SetSpeed = returnSpeed; // move away + motor1PrevCounts = 0; // set previous count to prevent speed spike + motor1CalSpeed = -motor1CalSpeed; // move away springHit = true; + looseTime = t.read(); // timer to compensate spring movement }else{ - if(springHit){ // if hit and after is no longer touching spring + // if hit and after is no longer touching spring and 0.5seconds passed + if(springHit && ((t.read() - looseTime) > 2)){ motor1SetSpeed = 0; springHit = false; + calibrating2 = true; // start calibrating 2 calibrating1 = false; - calibrating2 = true; // start calibrating 2 } } } if (calibrating2){ - motor2SetSpeed = motorCalSpeed; + motor2SetSpeed = motor2CalSpeed; redLed.write(1); greenLed.write(1); blueLed.write(0); - if(safetyIn.read() !=1){ // check if arm reached safety position + if(safetyIn.read() !=1 && !springHit){ // check if arm reached safety position encoder2.setPosition(0); // set motor 2 cal angle - motor2SetSpeed = returnSpeed; // move away + motor2PrevCounts = 0; // set previous cunt to prevent speed spike + motor2CalSpeed = -motor2CalSpeed; // move away springHit = true; + looseTime = t.read(); }else{ - if(springHit){ // if hit and after is no longer touching spring + if(springHit && ((t.read() - looseTime) > 2)){ motor2SetSpeed = 0; springHit = false; calibrating2 = false; // stop calibrating 2 } } } - now = t.read(); // call motor using timer instead of wait - if(now - lastCall > motorCall){ + // now = t.read(); // call motor using timer instead of wait + // if(now - lastCall > motorCall){ motorControl(); - lastCall = now; - } + wait(motorCall); // keep calling PID's with motorCall frequency + // lastCall = now; + // } } + redLed.write(0); greenLed.write(1); blueLed.write(1); motorsEnable = false; // turn motor's off again safetyOn = true; // turn safety on after callibration return true; // return true when finished