Austin Mitchell / Mbed 2 deprecated RobotFightingControl

Sword fighting robots WIP

Dependencies:   mbed PinDetect

meArm.cpp@0:e8eecd4b9a3d, 2018-12-06 (annotated)

Committer:
amitchell41
Date:
Thu Dec 06 01:30:51 2018 +0000
Revision:
0:e8eecd4b9a3d
Swing batta batta swing

Who changed what in which revision?

UserRevisionLine numberNew contents of line
amitchell41 0:e8eecd4b9a3d 1 /* meArm library York Hack Space May 2014
amitchell41 0:e8eecd4b9a3d 2 * A simple control library for Phenoptix' meArm
amitchell41 0:e8eecd4b9a3d 3 * Usage:
amitchell41 0:e8eecd4b9a3d 4 * meArm arm;
amitchell41 0:e8eecd4b9a3d 5 * arm.begin(1, 10, 9, 6);
amitchell41 0:e8eecd4b9a3d 6 * arm.openGripper();
amitchell41 0:e8eecd4b9a3d 7 * arm.gotoPoint(-80, 100, 140);
amitchell41 0:e8eecd4b9a3d 8 * arm.closeGripper();
amitchell41 0:e8eecd4b9a3d 9 * arm.gotoPoint(70, 200, 10);
amitchell41 0:e8eecd4b9a3d 10 * arm.openGripper();
amitchell41 0:e8eecd4b9a3d 11 */
amitchell41 0:e8eecd4b9a3d 12
amitchell41 0:e8eecd4b9a3d 13 //Convert from radians to degrees.
amitchell41 0:e8eecd4b9a3d 14 #define toDegrees(x) (x * 57.2957795)
amitchell41 0:e8eecd4b9a3d 15 //Convert from degrees to radians.
amitchell41 0:e8eecd4b9a3d 16 #define toRadians(x) (x * 0.01745329252)
amitchell41 0:e8eecd4b9a3d 17
amitchell41 0:e8eecd4b9a3d 18 #include "mbed.h"
amitchell41 0:e8eecd4b9a3d 19 #include "ik.h"
amitchell41 0:e8eecd4b9a3d 20 #include "meArm.h"
amitchell41 0:e8eecd4b9a3d 21 #include "Servo.h"
amitchell41 0:e8eecd4b9a3d 22
amitchell41 0:e8eecd4b9a3d 23 Servo _base(p21);
amitchell41 0:e8eecd4b9a3d 24 Servo _shoulder(p22);
amitchell41 0:e8eecd4b9a3d 25 Servo _elbow(p23);
amitchell41 0:e8eecd4b9a3d 26 Servo _gripper(p24);
amitchell41 0:e8eecd4b9a3d 27
amitchell41 0:e8eecd4b9a3d 28 bool setup_servo (ServoInfo& svo, const int n_min, const int n_max,
amitchell41 0:e8eecd4b9a3d 29 const float a_min, const float a_max)
amitchell41 0:e8eecd4b9a3d 30 {
amitchell41 0:e8eecd4b9a3d 31 float n_range = n_max - n_min;
amitchell41 0:e8eecd4b9a3d 32 float a_range = a_max - a_min;
amitchell41 0:e8eecd4b9a3d 33
amitchell41 0:e8eecd4b9a3d 34 // Must have a non-zero angle range
amitchell41 0:e8eecd4b9a3d 35 if(a_range == 0) return false;
amitchell41 0:e8eecd4b9a3d 36
amitchell41 0:e8eecd4b9a3d 37 // Calculate gain and zero
amitchell41 0:e8eecd4b9a3d 38 svo.gain = n_range / a_range;
amitchell41 0:e8eecd4b9a3d 39 svo.zero = n_min - svo.gain * a_min;
amitchell41 0:e8eecd4b9a3d 40
amitchell41 0:e8eecd4b9a3d 41 // Set limits
amitchell41 0:e8eecd4b9a3d 42 svo.n_min = n_min;
amitchell41 0:e8eecd4b9a3d 43 svo.n_max = n_max;
amitchell41 0:e8eecd4b9a3d 44
amitchell41 0:e8eecd4b9a3d 45 return true;
amitchell41 0:e8eecd4b9a3d 46 }
amitchell41 0:e8eecd4b9a3d 47
amitchell41 0:e8eecd4b9a3d 48 int angle2pwm (const ServoInfo& svo, const float angle)
amitchell41 0:e8eecd4b9a3d 49 {
amitchell41 0:e8eecd4b9a3d 50 float pwm = 0.5f + svo.zero + svo.gain * angle;
amitchell41 0:e8eecd4b9a3d 51 return int(pwm);
amitchell41 0:e8eecd4b9a3d 52 }
amitchell41 0:e8eecd4b9a3d 53
amitchell41 0:e8eecd4b9a3d 54 //Full constructor with calibration data
amitchell41 0:e8eecd4b9a3d 55 meArm::meArm(int sweepMinBase, int sweepMaxBase, float angleMinBase, float angleMaxBase,
amitchell41 0:e8eecd4b9a3d 56 int sweepMinShoulder, int sweepMaxShoulder, float angleMinShoulder, float angleMaxShoulder,
amitchell41 0:e8eecd4b9a3d 57 int sweepMinElbow, int sweepMaxElbow, float angleMinElbow, float angleMaxElbow,
amitchell41 0:e8eecd4b9a3d 58 int sweepMinGripper, int sweepMaxGripper, float angleMinGripper, float angleMaxGripper)
amitchell41 0:e8eecd4b9a3d 59 {
amitchell41 0:e8eecd4b9a3d 60 setup_servo(_svoBase, sweepMinBase, sweepMaxBase, angleMinBase, angleMaxBase);
amitchell41 0:e8eecd4b9a3d 61 setup_servo(_svoShoulder, sweepMinShoulder, sweepMaxShoulder, angleMinShoulder, angleMaxShoulder);
amitchell41 0:e8eecd4b9a3d 62 setup_servo(_svoElbow, sweepMinElbow, sweepMaxElbow, angleMinElbow, angleMaxElbow);
amitchell41 0:e8eecd4b9a3d 63 setup_servo(_svoGripper, sweepMinGripper, sweepMaxGripper, angleMinGripper, angleMaxGripper);
amitchell41 0:e8eecd4b9a3d 64 }
amitchell41 0:e8eecd4b9a3d 65
amitchell41 0:e8eecd4b9a3d 66 void meArm::begin() {
amitchell41 0:e8eecd4b9a3d 67 printf("Begun!");
amitchell41 0:e8eecd4b9a3d 68 goDirectlyTo(0, 100, 50);
amitchell41 0:e8eecd4b9a3d 69 }
amitchell41 0:e8eecd4b9a3d 70
amitchell41 0:e8eecd4b9a3d 71
amitchell41 0:e8eecd4b9a3d 72 //Set servos to reach a certain point directly without caring how we get there
amitchell41 0:e8eecd4b9a3d 73 void meArm::goDirectlyTo(float x, float y, float z) {
amitchell41 0:e8eecd4b9a3d 74 float radBase,radShoulder,radElbow;
amitchell41 0:e8eecd4b9a3d 75 if (solve(x, y, z, radBase, radShoulder, radElbow)) {
amitchell41 0:e8eecd4b9a3d 76 _base.position(angle2pwm(_svoBase,toDegrees(radBase))/100);
amitchell41 0:e8eecd4b9a3d 77 _shoulder.position(angle2pwm(_svoShoulder,toDegrees(radShoulder))/100);
amitchell41 0:e8eecd4b9a3d 78 _elbow.position(angle2pwm(_svoElbow,toDegrees(radElbow))/100);
amitchell41 0:e8eecd4b9a3d 79 _x = x; _y = y; _z = z;
amitchell41 0:e8eecd4b9a3d 80 }
amitchell41 0:e8eecd4b9a3d 81 }
amitchell41 0:e8eecd4b9a3d 82
amitchell41 0:e8eecd4b9a3d 83 //Travel smoothly from current point to another point
amitchell41 0:e8eecd4b9a3d 84 void meArm::gotoPoint(float x, float y, float z) {
amitchell41 0:e8eecd4b9a3d 85 //Starting points - current pos
amitchell41 0:e8eecd4b9a3d 86 float x0 = _x;
amitchell41 0:e8eecd4b9a3d 87 float y0 = _y;
amitchell41 0:e8eecd4b9a3d 88 float z0 = _z;
amitchell41 0:e8eecd4b9a3d 89 float dist = sqrt((x0-x)*(x0-x)+(y0-y)*(y0-y)+(z0-z)*(z0-z));
amitchell41 0:e8eecd4b9a3d 90 int step = 5;
amitchell41 0:e8eecd4b9a3d 91 for (int i = 0; i<dist; i+= step) {
amitchell41 0:e8eecd4b9a3d 92 goDirectlyTo(x0 + (x-x0)*i/dist, y0 + (y-y0) * i/dist, z0 + (z-z0) * i/dist);
amitchell41 0:e8eecd4b9a3d 93 }
amitchell41 0:e8eecd4b9a3d 94 goDirectlyTo(x, y, z);
amitchell41 0:e8eecd4b9a3d 95 wait(1);
amitchell41 0:e8eecd4b9a3d 96 }
amitchell41 0:e8eecd4b9a3d 97
amitchell41 0:e8eecd4b9a3d 98 //Get x and y from theta and r
amitchell41 0:e8eecd4b9a3d 99 void meArm::polarToCartesian(float theta, float r, float& x, float& y){
amitchell41 0:e8eecd4b9a3d 100 _r = r;
amitchell41 0:e8eecd4b9a3d 101 _t = theta;
amitchell41 0:e8eecd4b9a3d 102 x = r*sin(theta);
amitchell41 0:e8eecd4b9a3d 103 y = r*cos(theta);
amitchell41 0:e8eecd4b9a3d 104 }
amitchell41 0:e8eecd4b9a3d 105
amitchell41 0:e8eecd4b9a3d 106 //Same as above but for cylindrical polar coodrinates
amitchell41 0:e8eecd4b9a3d 107 void meArm::gotoPointCylinder(float theta, float r, float z){
amitchell41 0:e8eecd4b9a3d 108 float x, y;
amitchell41 0:e8eecd4b9a3d 109 polarToCartesian(theta, r, x, y);
amitchell41 0:e8eecd4b9a3d 110 gotoPoint(x,y,z);
amitchell41 0:e8eecd4b9a3d 111 }
amitchell41 0:e8eecd4b9a3d 112
amitchell41 0:e8eecd4b9a3d 113 void meArm::goDirectlyToCylinder(float theta, float r, float z){
amitchell41 0:e8eecd4b9a3d 114 float x, y;
amitchell41 0:e8eecd4b9a3d 115 polarToCartesian(theta, r, x, y);
amitchell41 0:e8eecd4b9a3d 116 goDirectlyTo(x,y,z);
amitchell41 0:e8eecd4b9a3d 117 }
amitchell41 0:e8eecd4b9a3d 118
amitchell41 0:e8eecd4b9a3d 119 //Check to see if possible
amitchell41 0:e8eecd4b9a3d 120 bool meArm::isReachable(float x, float y, float z) {
amitchell41 0:e8eecd4b9a3d 121 float radBase,radShoulder,radElbow;
amitchell41 0:e8eecd4b9a3d 122 return (solve(x, y, z, radBase, radShoulder, radElbow));
amitchell41 0:e8eecd4b9a3d 123 }
amitchell41 0:e8eecd4b9a3d 124
amitchell41 0:e8eecd4b9a3d 125 //Grab something
amitchell41 0:e8eecd4b9a3d 126 void meArm::openGripper() {
amitchell41 0:e8eecd4b9a3d 127 _gripper.write(1.0);
amitchell41 0:e8eecd4b9a3d 128 wait(0.5);
amitchell41 0:e8eecd4b9a3d 129 }
amitchell41 0:e8eecd4b9a3d 130
amitchell41 0:e8eecd4b9a3d 131 //Let go of something
amitchell41 0:e8eecd4b9a3d 132 void meArm::closeGripper() {
amitchell41 0:e8eecd4b9a3d 133 _gripper.write(0.0);
amitchell41 0:e8eecd4b9a3d 134 wait(0.5);
amitchell41 0:e8eecd4b9a3d 135 }
amitchell41 0:e8eecd4b9a3d 136
amitchell41 0:e8eecd4b9a3d 137 //Current x, y and z
amitchell41 0:e8eecd4b9a3d 138 float meArm::getX() {
amitchell41 0:e8eecd4b9a3d 139 return _x;
amitchell41 0:e8eecd4b9a3d 140 }
amitchell41 0:e8eecd4b9a3d 141 float meArm::getY() {
amitchell41 0:e8eecd4b9a3d 142 return _y;
amitchell41 0:e8eecd4b9a3d 143 }
amitchell41 0:e8eecd4b9a3d 144 float meArm::getZ() {
amitchell41 0:e8eecd4b9a3d 145 return _z;
amitchell41 0:e8eecd4b9a3d 146 }
amitchell41 0:e8eecd4b9a3d 147
amitchell41 0:e8eecd4b9a3d 148
amitchell41 0:e8eecd4b9a3d 149 float meArm::getR() {
amitchell41 0:e8eecd4b9a3d 150 return _r;
amitchell41 0:e8eecd4b9a3d 151 }
amitchell41 0:e8eecd4b9a3d 152 float meArm::getTheta() {
amitchell41 0:e8eecd4b9a3d 153 return _t;
amitchell41 0:e8eecd4b9a3d 154 }