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:
- 106:1773bf7b95c5
- Parent:
- 100:222c27f55b85
- Child:
- 107:de47331612d9
diff -r 222c27f55b85 -r 1773bf7b95c5 actuators.cpp --- a/actuators.cpp Thu Oct 22 08:51:50 2015 +0000 +++ b/actuators.cpp Mon Oct 26 12:46:29 2015 +0100 @@ -5,9 +5,13 @@ #include "encoder.h" #include "HIDScope.h" + // Motor control constants + #define pwm_frequency 50000 // still High, could be lowered + #define PI 3.14159265 + // functions for controlling the motors bool motorsEnable = false; - bool safetyOn = true; // start with safety off for calibration + bool safetyOn = true; double encoder1Counts = 0; @@ -25,8 +29,6 @@ double motor1SetSpeed = 0; double motor2SetSpeed = 0; - double servoPos = 0; - double motor1PWM = 0; double motor2PWM = 0; @@ -199,7 +201,7 @@ servo.pulsewidth(servoPulsewidth); } -void calibrateMotors(){ +bool calibrateMotors(){ safetyOn = false; // safety springs off motorsEnable = true; // motors on redLed.write(0); greenLed.write(1); blueLed.write(1); @@ -243,6 +245,7 @@ } motorsEnable = false; // turn motor's off again safetyOn = true; // turn safety on after callibration + return true; // return true wehn finished } @@ -253,3 +256,32 @@ } } } + + +bool kinematics(){ + // calculate current x and Y + X = L2*cos((motor1Pos + motor2Pos)*PI/180) + L1*cos(motor1Pos*PI/180); + Y = L2*sin((motor1Pos + motor2Pos)*PI/180) + L1*sin(motor1Pos*PI/180); + // check if x and y are within limits + // else Store the constraint line + // check if movement is in direction of constraint + // else return false no movement (anglespeed = 0) + // calculate required angle speeds + if( (X>Xmax && setXSpeed > 0 )|| \ + (X<Xmin && setXSpeed < 0 )|| \ + (Y>Ymax && setYSpeed > 0 )|| \ + (Y<Ymin && setYSpeed < 0 ) \ + ){ + motor1SetSpeed = 0; + motor2SetSpeed = 0; + return false; + break; + } +motor1SetSpeed = (setXSpeed*cos((motor1Pos + motor2Pos)*PI/180) + \ + setYSpeed*sin((motor1Pos + motor2Pos)*PI/180))/(L1*sin(motor2Pos*PI/180)); +motor2SetSpeed = -(setXSpeed*L2*cos((motor1Pos + motor2Pos)*PI/180) + \ + setYSpeed*L2*sin((motor1Pos + motor2Pos)*PI/180) + \ + setXSpeed*L1*cos(motor1Pos*PI/180) + \ + setYSpeed*L1*sin(motor1Pos*PI/180))/(L1*L2*sin(motor2Pos*PI/180)); + +} \ No newline at end of file