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:
- 124:f67ce69557db
- Parent:
- 121:6d8f1bdcda05
- Child:
- 127:831f03471efb
diff -r 6d8f1bdcda05 -r f67ce69557db actuators.cpp --- a/actuators.cpp Wed Oct 28 13:38:36 2015 +0100 +++ b/actuators.cpp Thu Oct 29 18:23:08 2015 +0100 @@ -38,8 +38,8 @@ double motor2PWM = 0; // Set PID values -double Kp1 = 1; -double Ki1 = 0; +double Kp1 = 0.008; +double Ki1 = 0.08; double Kd1 = 0; double Kp2 = 0.008; @@ -56,17 +56,6 @@ double servoSpeed = 0; const int servoStartPos = 1300; // Servo ranges from 600ms(-90) to 2000ms(90), 1300 is 0 deg -// Set calibration values -double motor1CalSpeed = -5; // deg/sec -double motor2CalSpeed = 5; -bool springHit = false; -float lastCall = 0; -bool calibrating1 = true; -bool calibrating2 = false; -double looseTime = 0; - -//bool calReady = false; - // Create object instances // Safety Pin DigitalIn safetyIn(safetyPin); @@ -122,10 +111,18 @@ void motorControl(){ // EMG signals to motor speeds - // const double scaleVel = 20; - // motor1SetSpeed = x_velocity*scaleVel; - // motor2SetSpeed = y_velocity*scaleVel; - // servoSpeed = z_velocity*scaleVel; + if(!usePotmeters && controlAngle){ + double scaleVel = 20; + motor1SetSpeed = x_velocity*scaleVel; + motor2SetSpeed = y_velocity*scaleVel; + servoSpeed = z_velocity*scaleVel; + } + if(!usePotmeters && controlDirection){ + double scaleVel = 0.1; + setXSpeed = x_velocity*scaleVel; + setYSpeed = y_velocity*scaleVel; + servoSpeed = z_velocity*scaleVel; + } // get encoder positions in degrees // 131.25:1 gear ratio // getPosition uses X2 configuration, so 32 counts per revolution @@ -134,7 +131,6 @@ encoder1Counts = encoder1.getPosition(); encoder2Counts = encoder2.getPosition(); - motor1Pos = ((encoder1Counts/32)/131.25)*360; motor2Pos = ((encoder2Counts/32)/131.25)*360; @@ -151,7 +147,7 @@ // calculate motor setpoint speed in deg/sec from setpoint x/y speed // exclude kinematics when still calibrating - if (calReady){ + if (calReady && !controlAngle){ kinematics(); } @@ -170,6 +166,9 @@ motor1PID.Compute(); // calculate PID outputs, output changes automatically motor2PID.Compute(); // write new values to motor's + if (motor1SetSpeed ==0 ){ + motor1PID.SetOutputLimits(0,0); + } if (motor1SetSpeed > 0 ){ // CCW rotation direction1 = true; motor1PID.SetOutputLimits(0,1); // change pid output direction @@ -177,6 +176,9 @@ direction1 = false; // CW rotation motor1PID.SetOutputLimits(-1,0); } + if (motor2SetSpeed ==0 ){ + motor2PID.SetOutputLimits(0,0); + } if (motor2SetSpeed > 0 ){ // CCW rotation direction2 = true; motor2PID.SetOutputLimits(0,1); @@ -196,7 +198,16 @@ } const double motor1StartPos = (-10*32*131.25)/360; // angle to encoder counts -const double motor2StartPos = (114*32*131.25)/360; +const double motor2StartPos = (120*32*131.25)/360; + +// Set calibration values +double motor1CalSpeed = -10; // deg/sec +double motor2CalSpeed = 5; +bool springHit = false; +float lastCall = 0; +bool calibrating1 = true; +bool calibrating2 = false; +double looseTime = 0; bool calibrateMotors(){ safetyOn = false; // safety springs off @@ -269,7 +280,7 @@ const double Ymax = 0.645; const double Ymin = 0.33; double Xpos = 0; // set values -double Ypos =0; +double Ypos = 0; @@ -291,12 +302,12 @@ motor2SetSpeed = 0; return false; } -motor1SetSpeed = (setXSpeed*cos((motor1Pos + motor2Pos)*PI/180) + \ - setYSpeed*sin((motor1Pos + motor2Pos)*PI/180))/(L1*sin(motor2Pos*PI/180)); -motor2SetSpeed = -(setXSpeed*(L2+L3)*cos((motor1Pos + motor2Pos)*PI/180) + \ +motor1SetSpeed = (180/PI)*((setXSpeed*cos((motor1Pos + motor2Pos)*PI/180) + \ + setYSpeed*sin((motor1Pos + motor2Pos)*PI/180))/(L1*sin(motor2Pos*PI/180))); +motor2SetSpeed = (180/PI)*(-(setXSpeed*(L2+L3)*cos((motor1Pos + motor2Pos)*PI/180) + \ setYSpeed*(L2+L3)*sin((motor1Pos + motor2Pos)*PI/180) + \ setXSpeed*L1*cos(motor1Pos*PI/180) + \ - setYSpeed*L1*sin(motor1Pos*PI/180))/(L1*(L2+L3)*sin(motor2Pos*PI/180)); + setYSpeed*L1*sin(motor1Pos*PI/180))/(L1*(L2+L3)*sin(motor2Pos*PI/180))); return true; } \ No newline at end of file