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:
- 115:d8d2968981f3
- Parent:
- 110:a6439e13be8b
- Child:
- 116:8b812e268b85
diff -r a6439e13be8b -r d8d2968981f3 actuators.cpp --- a/actuators.cpp Tue Oct 27 14:45:15 2015 +0000 +++ b/actuators.cpp Tue Oct 27 16:21:20 2015 +0100 @@ -54,8 +54,8 @@ 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 motorCalSpeed = 5; // deg/sec +double returnSpeed = -5; bool springHit = false; float lastCall = 0; bool calibrating1 = true; @@ -115,13 +115,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 @@ -191,20 +189,21 @@ safetyOn = false; // safety springs off motorsEnable = true; // motors on redLed.write(0); greenLed.write(1); blueLed.write(1); - while (calibrating1 || calibrating2){ + if (calibrating1 || calibrating2){ if (calibrating1){ motor1SetSpeed = motorCalSpeed; redLed.write(1); greenLed.write(0); blueLed.write(1); if(safetyIn.read() !=1){ // check if arm reached safety position encoder1.setPosition(0); // set motor 1 cal angle + motor1PrevCounts = 0; // set previous count to prevent speed spike motor1SetSpeed = returnSpeed; // move away springHit = true; }else{ if(springHit){ // if hit and after is no longer touching spring motor1SetSpeed = 0; springHit = false; + calibrating2 = true; // start calibrating 2 calibrating1 = false; - calibrating2 = true; // start calibrating 2 } } } @@ -213,6 +212,7 @@ redLed.write(1); greenLed.write(1); blueLed.write(0); if(safetyIn.read() !=1){ // check if arm reached safety position encoder2.setPosition(0); // set motor 2 cal angle + motor2PrevCounts = 0; // set previous cunt to prevent speed spike motor2SetSpeed = returnSpeed; // move away springHit = true; }else{ @@ -224,10 +224,11 @@ } } now = t.read(); // call motor using timer instead of wait - if(now - lastCall > motorCall){ + // if(now - lastCall > motorCall){ motorControl(); - lastCall = now; - } + wait(motorCall); + // lastCall = now; + // } } motorsEnable = false; // turn motor's off again