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:
- 122:1a5df0765790
- Parent:
- 121:6d8f1bdcda05
- Child:
- 123:815ace09b753
diff -r 6d8f1bdcda05 -r 1a5df0765790 actuators.cpp --- a/actuators.cpp Wed Oct 28 13:38:36 2015 +0100 +++ b/actuators.cpp Thu Oct 29 12:26:50 2015 +0000 @@ -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); @@ -151,13 +140,16 @@ // calculate motor setpoint speed in deg/sec from setpoint x/y speed // exclude kinematics when still calibrating - if (calReady){ - kinematics(); - } +// if (calReady){ +// kinematics(); +// } if(motorsEnable){ // only run motors if switch is enabled // compute new PID parameters using setpoint angle speeds and encoder speed - writeMotors(); + motor1Dir.write(true); + motor2Dir.write(true); + motor1.write(0); + motor2.write(0.05f); servoControl(); }else{ // write 0 to motors @@ -196,7 +188,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 +270,7 @@ const double Ymax = 0.645; const double Ymin = 0.33; double Xpos = 0; // set values -double Ypos =0; +double Ypos = 0; @@ -291,12 +292,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