Kim Bruil / Mbed 2 deprecated r_robot

robot positie error test ding

Dependencies:   MODSERIAL mbed EMG_Input QEI biquadFilter

main.cpp@1:ffc1a2d6a824, 2016-10-24 (annotated)

Committer:
kbruil
Date:
Mon Oct 24 19:27:39 2016 +0000
Revision:
1:ffc1a2d6a824
Parent:
0:86fe02a64f0e
Child:
2:027d19cda97b
resolving annoying atan2 and acos issues;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kbruil 0:86fe02a64f0e 1 #include "mbed.h"
kbruil 0:86fe02a64f0e 2 #include "math.h"
kbruil 0:86fe02a64f0e 3 #include "stdio.h"
kbruil 0:86fe02a64f0e 4 #include "MODSERIAL.h"
kbruil 0:86fe02a64f0e 5 // Angle limits 215, 345 lower arm, 0, 145 upper arm
kbruil 0:86fe02a64f0e 6 // Robot arm x,y limits (limit to table top)
kbruil 0:86fe02a64f0e 7
kbruil 0:86fe02a64f0e 8
kbruil 0:86fe02a64f0e 9 #define M_PI 3.141592653589793238462643383279502884L // Pi
kbruil 1:ffc1a2d6a824 10 const double R_XTABLE_LO = -30.0; // Robot in the middle, table width=60
kbruil 1:ffc1a2d6a824 11 const double R_XTABLE_HI = 30.0;
kbruil 1:ffc1a2d6a824 12 const double R_YTABLE_LO = -18.0;
kbruil 1:ffc1a2d6a824 13 const double R_YTABLE_HI = 82.0;
kbruil 1:ffc1a2d6a824 14 const double R_XROBOT_LO = -8.0; // Robot limited from moving onto itself (20x20 base)
kbruil 1:ffc1a2d6a824 15 const double R_XROBOT_HI = 8.0;
kbruil 1:ffc1a2d6a824 16 const double R_YROBOT_LO = -18.0;
kbruil 1:ffc1a2d6a824 17 const double R_YROBOT_HI = 2.0;
kbruil 1:ffc1a2d6a824 18
kbruil 1:ffc1a2d6a824 19 const double ARM1_ALIMLO = (2*M_PI/360)*125;
kbruil 1:ffc1a2d6a824 20 const double ARM1_ALIMHI = -(2*M_PI/360)*125;
kbruil 1:ffc1a2d6a824 21 const double ARM2_ALIMLO = 0;
kbruil 1:ffc1a2d6a824 22 const double ARM2_ALIMHI = (2*M_PI/360)*145;
kbruil 0:86fe02a64f0e 23
kbruil 0:86fe02a64f0e 24 // Struct r_link:
kbruil 0:86fe02a64f0e 25 // Defines a robot link type (arm or end effector).
kbruil 0:86fe02a64f0e 26 struct r_link {
kbruil 0:86fe02a64f0e 27 float length; // link length
kbruil 0:86fe02a64f0e 28 float x; // link x position
kbruil 0:86fe02a64f0e 29 float y; // link y position
kbruil 0:86fe02a64f0e 30 float vx; // link x speed
kbruil 0:86fe02a64f0e 31 float vy; // link y speed
kbruil 0:86fe02a64f0e 32 float ax; // link x accelleration
kbruil 0:86fe02a64f0e 33 float ay; // link y accelleration
kbruil 0:86fe02a64f0e 34 float theta; // link angle
kbruil 0:86fe02a64f0e 35 float omega; // link angular velocity
kbruil 0:86fe02a64f0e 36 float alpha; // link angular accelleration
kbruil 0:86fe02a64f0e 37 };
kbruil 0:86fe02a64f0e 38
kbruil 0:86fe02a64f0e 39 MODSERIAL pc(USBTX, USBRX);
kbruil 0:86fe02a64f0e 40 DigitalIn switch1(SW3);
kbruil 0:86fe02a64f0e 41 DigitalIn switch2(SW2);
kbruil 0:86fe02a64f0e 42 DigitalIn switch3(D7);
kbruil 0:86fe02a64f0e 43 DigitalIn switch4(D9);
kbruil 0:86fe02a64f0e 44 DigitalOut ledr(LED_RED);
kbruil 0:86fe02a64f0e 45 DigitalOut ledg(LED_GREEN);
kbruil 0:86fe02a64f0e 46
kbruil 0:86fe02a64f0e 47
kbruil 0:86fe02a64f0e 48 r_link arm1; // Robot upper arm
kbruil 0:86fe02a64f0e 49 r_link arm2; // Robot lower arm
kbruil 0:86fe02a64f0e 50 r_link end; // Robot end effector
kbruil 0:86fe02a64f0e 51
kbruil 0:86fe02a64f0e 52 // robot_applylinmits
kbruil 0:86fe02a64f0e 53 // Input: x,y
kbruil 0:86fe02a64f0e 54 // Output: x,y limited to limits set by constants
kbruil 0:86fe02a64f0e 55 void robot_applylimits (float& x, float& y){
kbruil 0:86fe02a64f0e 56 float z;
kbruil 0:86fe02a64f0e 57 float angle;
kbruil 0:86fe02a64f0e 58
kbruil 0:86fe02a64f0e 59 // Limit x,y to table top size
kbruil 0:86fe02a64f0e 60 if (x<R_XTABLE_LO) { x=R_XTABLE_LO; }
kbruil 0:86fe02a64f0e 61 else if (x>R_XTABLE_HI) { x=R_XTABLE_HI; }
kbruil 0:86fe02a64f0e 62 if (y<R_YTABLE_LO) { y=R_YTABLE_LO; }
kbruil 0:86fe02a64f0e 63 else if (y>R_YTABLE_HI) { y=R_YTABLE_HI; }
kbruil 0:86fe02a64f0e 64
kbruil 0:86fe02a64f0e 65 // Limit x,y from moving into the robot base
kbruil 0:86fe02a64f0e 66 /*
kbruil 0:86fe02a64f0e 67 if (x>R_XROBOT_LO && x<R_XROBOT_HI && y>R_YROBOT_LO && y<R_YROBOT_HI)
kbruil 0:86fe02a64f0e 68 {
kbruil 0:86fe02a64f0e 69 if (x<=0){
kbruil 0:86fe02a64f0e 70 x=R_XROBOT_LO;
kbruil 0:86fe02a64f0e 71 }
kbruil 0:86fe02a64f0e 72 else {
kbruil 0:86fe02a64f0e 73 x=R_XROBOT_HI;
kbruil 0:86fe02a64f0e 74 }
kbruil 1:ffc1a2d6a824 75 if (y>R_YROBOT_LO && y<R_YROBOT_HI){
kbruil 0:86fe02a64f0e 76 y=R_YROBOT_HI;
kbruil 0:86fe02a64f0e 77 }
kbruil 0:86fe02a64f0e 78 }
kbruil 0:86fe02a64f0e 79 */
kbruil 0:86fe02a64f0e 80 // Limit x,y to maximum size of robot arm
kbruil 0:86fe02a64f0e 81 z = sqrt(pow(x,2)+pow(y,2));
kbruil 0:86fe02a64f0e 82 angle = atan2(y,x);
kbruil 1:ffc1a2d6a824 83 // angle = angle + M_PI;
kbruil 0:86fe02a64f0e 84 if (z>(arm1.length+arm2.length)){
kbruil 0:86fe02a64f0e 85 z = arm1.length+arm2.length;
kbruil 0:86fe02a64f0e 86 x = z*cos(angle);
kbruil 0:86fe02a64f0e 87 y = z*sin(angle);
kbruil 0:86fe02a64f0e 88 }
kbruil 0:86fe02a64f0e 89 }
kbruil 0:86fe02a64f0e 90
kbruil 0:86fe02a64f0e 91 // robot_setposition:
kbruil 0:86fe02a64f0e 92 // Input: x,y cartesian coordinates
kbruil 0:86fe02a64f0e 93 // Function: calculate arm angles based on cartesian coordinates
kbruil 0:86fe02a64f0e 94 void robot_setposition (float x,float y){
kbruil 0:86fe02a64f0e 95 float z;
kbruil 0:86fe02a64f0e 96 float z1;
kbruil 0:86fe02a64f0e 97 float z2;
kbruil 0:86fe02a64f0e 98
kbruil 0:86fe02a64f0e 99 float angle;
kbruil 0:86fe02a64f0e 100 float b1,b2;
kbruil 0:86fe02a64f0e 101
kbruil 0:86fe02a64f0e 102 float old;
kbruil 0:86fe02a64f0e 103
kbruil 0:86fe02a64f0e 104 robot_applylimits(x,y);
kbruil 0:86fe02a64f0e 105 // end.x = x;
kbruil 0:86fe02a64f0e 106 // end.y = y;
kbruil 0:86fe02a64f0e 107
kbruil 1:ffc1a2d6a824 108 // printf("xy: %f, %f\n\r",x,y);
kbruil 0:86fe02a64f0e 109 z = sqrt(pow(x,2)+pow(y,2));
kbruil 0:86fe02a64f0e 110 angle = atan2(y,x);
kbruil 1:ffc1a2d6a824 111
kbruil 1:ffc1a2d6a824 112 // printf("angle voor gekloot: %f\n\r", angle);
kbruil 1:ffc1a2d6a824 113
kbruil 1:ffc1a2d6a824 114 if (angle<0.5*M_PI && angle>0){
kbruil 1:ffc1a2d6a824 115 angle = angle - 0.5*M_PI;
kbruil 0:86fe02a64f0e 116 }
kbruil 1:ffc1a2d6a824 117 else if (angle<M_PI && angle>0.5*M_PI){
kbruil 1:ffc1a2d6a824 118 angle = angle - 0.5*M_PI;
kbruil 0:86fe02a64f0e 119 }
kbruil 1:ffc1a2d6a824 120 else if (angle<0 && angle>-0.5*M_PI){
kbruil 1:ffc1a2d6a824 121 angle = angle - 0.5 * M_PI;
kbruil 1:ffc1a2d6a824 122 }
kbruil 1:ffc1a2d6a824 123 else{
kbruil 1:ffc1a2d6a824 124 angle = angle + 1.5*M_PI;
kbruil 1:ffc1a2d6a824 125 }
kbruil 1:ffc1a2d6a824 126 if (y==0 && x<0) { angle = 0.5*M_PI; }
kbruil 0:86fe02a64f0e 127
kbruil 0:86fe02a64f0e 128 z1 = (pow(arm1.length,2)-pow(arm2.length,2)+pow(z,2))/(2*z);
kbruil 0:86fe02a64f0e 129 z2 = z-z1;
kbruil 1:ffc1a2d6a824 130 printf("z1 and z2: %f, %f\n\r", z1, z2);
kbruil 1:ffc1a2d6a824 131 if (z1>arm1.length) {z1 = arm1.length;}
kbruil 1:ffc1a2d6a824 132 else if (z1<-arm1.length) { z1 = -arm1.length;}
kbruil 1:ffc1a2d6a824 133 if (z2>arm2.length) {z2 = arm2.length;}
kbruil 1:ffc1a2d6a824 134 else if (z2<-arm2.length) { z2 = -arm2.length;}
kbruil 0:86fe02a64f0e 135
kbruil 0:86fe02a64f0e 136 b1 = acos(z1/arm1.length);
kbruil 0:86fe02a64f0e 137 b2 = acos(z2/arm2.length);
kbruil 0:86fe02a64f0e 138
kbruil 0:86fe02a64f0e 139 old = arm1.theta;
kbruil 0:86fe02a64f0e 140 arm1.theta = angle - b1;
kbruil 0:86fe02a64f0e 141
kbruil 1:ffc1a2d6a824 142 if (arm1.theta>ARM1_ALIMLO){
kbruil 0:86fe02a64f0e 143 arm1.theta = ARM1_ALIMLO;
kbruil 0:86fe02a64f0e 144 }
kbruil 0:86fe02a64f0e 145 else if (arm1.theta<ARM1_ALIMHI){
kbruil 0:86fe02a64f0e 146 arm1.theta = ARM1_ALIMHI;
kbruil 0:86fe02a64f0e 147 }
kbruil 1:ffc1a2d6a824 148
kbruil 0:86fe02a64f0e 149 arm1.omega = arm1.theta - old;
kbruil 0:86fe02a64f0e 150 old = arm2.theta;
kbruil 0:86fe02a64f0e 151 arm2.theta = b1 + b2;
kbruil 0:86fe02a64f0e 152 if (arm2.theta>ARM2_ALIMHI){
kbruil 0:86fe02a64f0e 153 arm2.theta = ARM2_ALIMHI;
kbruil 1:ffc1a2d6a824 154 } else if (arm2.theta < 0) {
kbruil 1:ffc1a2d6a824 155 arm2.theta = 0;
kbruil 0:86fe02a64f0e 156 }
kbruil 1:ffc1a2d6a824 157
kbruil 1:ffc1a2d6a824 158 /*
kbruil 0:86fe02a64f0e 159 arm2.omega = arm2.theta - old;
kbruil 0:86fe02a64f0e 160 old = arm2.x;
kbruil 0:86fe02a64f0e 161 arm2.x = arm1.length*-sin(arm1.theta);
kbruil 0:86fe02a64f0e 162 arm2.vx = arm2.x - old;
kbruil 0:86fe02a64f0e 163 old = arm2.y;
kbruil 0:86fe02a64f0e 164 arm2.y = arm1.length*cos(arm1.theta);
kbruil 0:86fe02a64f0e 165 arm2.vy = arm2.y - old;
kbruil 0:86fe02a64f0e 166
kbruil 0:86fe02a64f0e 167 old = end.x;
kbruil 0:86fe02a64f0e 168 end.x = arm2.length*(-sin(arm1.theta+arm2.theta)) + arm2.x;
kbruil 0:86fe02a64f0e 169 end.vx = end.x - old;
kbruil 0:86fe02a64f0e 170 old = end.y;
kbruil 0:86fe02a64f0e 171 end.y = arm2.length*(cos(arm1.theta+arm2.theta)) + arm2.y;
kbruil 0:86fe02a64f0e 172 end.vy = old - end.y;
kbruil 1:ffc1a2d6a824 173 */
kbruil 1:ffc1a2d6a824 174 end.x = x;
kbruil 1:ffc1a2d6a824 175 end.y = y;
kbruil 1:ffc1a2d6a824 176
kbruil 0:86fe02a64f0e 177 }
kbruil 0:86fe02a64f0e 178
kbruil 0:86fe02a64f0e 179 // robot_init
kbruil 0:86fe02a64f0e 180 // Initialise robot
kbruil 0:86fe02a64f0e 181 void robot_init() {
kbruil 0:86fe02a64f0e 182 // Init arm1 (upper arm) first link
kbruil 0:86fe02a64f0e 183 arm1.length = 28.0f;
kbruil 0:86fe02a64f0e 184 arm1.x = 0; arm1.y = 0;
kbruil 0:86fe02a64f0e 185 arm1.vx = 0; arm1.vy = 0;
kbruil 0:86fe02a64f0e 186 arm1.ax = 0; arm1.ay = 0;
kbruil 1:ffc1a2d6a824 187 arm1.theta = 0;
kbruil 0:86fe02a64f0e 188 arm1.omega = 0;
kbruil 0:86fe02a64f0e 189 arm1.alpha = 0;
kbruil 0:86fe02a64f0e 190
kbruil 0:86fe02a64f0e 191 // Init arm2 (lower arm) second link
kbruil 0:86fe02a64f0e 192 arm2.length = 35.0f;
kbruil 0:86fe02a64f0e 193 arm2.x = 0; arm2.y = arm1.length;
kbruil 0:86fe02a64f0e 194 arm2.vx = 0; arm2.vy = 0;
kbruil 0:86fe02a64f0e 195 arm2.ax = 0; arm2.ay = 0;
kbruil 1:ffc1a2d6a824 196 arm2.theta = 0;
kbruil 0:86fe02a64f0e 197 arm2.omega = 0;
kbruil 0:86fe02a64f0e 198 arm2.alpha = 0;
kbruil 0:86fe02a64f0e 199
kbruil 0:86fe02a64f0e 200 // Init end (end effector) third/last link
kbruil 0:86fe02a64f0e 201 end.length = 30.0f;
kbruil 0:86fe02a64f0e 202 end.x = 0; end.y = arm1.length+arm2.length;
kbruil 0:86fe02a64f0e 203 end.vx = 0; end.vy = 0;
kbruil 0:86fe02a64f0e 204 end.ax = 0; end.ay = 0;
kbruil 0:86fe02a64f0e 205 end.theta = 0;
kbruil 0:86fe02a64f0e 206 end.omega = 0;
kbruil 0:86fe02a64f0e 207 end.alpha = 0;
kbruil 0:86fe02a64f0e 208 }
kbruil 0:86fe02a64f0e 209
kbruil 0:86fe02a64f0e 210 int lr_state=0;
kbruil 0:86fe02a64f0e 211 int ud_state=0;
kbruil 0:86fe02a64f0e 212 int sw2_state=0;
kbruil 0:86fe02a64f0e 213 float ax=0.1;
kbruil 0:86fe02a64f0e 214 float ay=0.1;
kbruil 0:86fe02a64f0e 215 float vx=0;
kbruil 0:86fe02a64f0e 216 float vy=0;
kbruil 1:ffc1a2d6a824 217 float spd=1.0;
kbruil 0:86fe02a64f0e 218
kbruil 0:86fe02a64f0e 219 void inputswitches()
kbruil 0:86fe02a64f0e 220 {
kbruil 1:ffc1a2d6a824 221 /* if(switch1.read())
kbruil 0:86fe02a64f0e 222 {
kbruil 0:86fe02a64f0e 223 if (lr_state==0){
kbruil 0:86fe02a64f0e 224 vx=0;
kbruil 0:86fe02a64f0e 225 ledr.write(1);
kbruil 0:86fe02a64f0e 226 }
kbruil 0:86fe02a64f0e 227 }
kbruil 0:86fe02a64f0e 228 else
kbruil 0:86fe02a64f0e 229 {
kbruil 0:86fe02a64f0e 230 lr_state=0;
kbruil 0:86fe02a64f0e 231 vx = vx - ax;
kbruil 0:86fe02a64f0e 232 if (vx<-3.0f){
kbruil 0:86fe02a64f0e 233 vx=-3.0f;
kbruil 0:86fe02a64f0e 234 }
kbruil 0:86fe02a64f0e 235 ledr.write(0);
kbruil 0:86fe02a64f0e 236 }
kbruil 0:86fe02a64f0e 237
kbruil 0:86fe02a64f0e 238 if(switch2.read())
kbruil 0:86fe02a64f0e 239 {
kbruil 0:86fe02a64f0e 240 if(lr_state==1){
kbruil 0:86fe02a64f0e 241 vx=0;
kbruil 0:86fe02a64f0e 242 ledr.write(1);
kbruil 0:86fe02a64f0e 243 }
kbruil 0:86fe02a64f0e 244 }
kbruil 0:86fe02a64f0e 245 else
kbruil 0:86fe02a64f0e 246 {
kbruil 0:86fe02a64f0e 247 lr_state=1;
kbruil 0:86fe02a64f0e 248 vx = vx + ax;
kbruil 0:86fe02a64f0e 249 if (vx>3.0f){
kbruil 0:86fe02a64f0e 250 vx=3.0f;
kbruil 0:86fe02a64f0e 251 }
kbruil 0:86fe02a64f0e 252 ledr.write(0);
kbruil 0:86fe02a64f0e 253 }
kbruil 0:86fe02a64f0e 254 if(switch3.read())
kbruil 0:86fe02a64f0e 255 {
kbruil 0:86fe02a64f0e 256 if (ud_state==0){
kbruil 0:86fe02a64f0e 257 vy=0;
kbruil 0:86fe02a64f0e 258 ledr.write(1);
kbruil 0:86fe02a64f0e 259 }
kbruil 0:86fe02a64f0e 260 }
kbruil 0:86fe02a64f0e 261 else
kbruil 0:86fe02a64f0e 262 {
kbruil 0:86fe02a64f0e 263 ud_state=0;
kbruil 0:86fe02a64f0e 264 vy = vy - ay;
kbruil 0:86fe02a64f0e 265 if (vy<-3.0f){
kbruil 0:86fe02a64f0e 266 vy=-3.0f;
kbruil 0:86fe02a64f0e 267 }
kbruil 0:86fe02a64f0e 268 ledr.write(0);
kbruil 0:86fe02a64f0e 269 }
kbruil 0:86fe02a64f0e 270
kbruil 0:86fe02a64f0e 271 if(switch4.read())
kbruil 0:86fe02a64f0e 272 {
kbruil 0:86fe02a64f0e 273 if(ud_state==1){
kbruil 0:86fe02a64f0e 274 vy=0;
kbruil 0:86fe02a64f0e 275 ledr.write(1);
kbruil 0:86fe02a64f0e 276 }
kbruil 0:86fe02a64f0e 277 }
kbruil 0:86fe02a64f0e 278 else
kbruil 0:86fe02a64f0e 279 {
kbruil 0:86fe02a64f0e 280 ud_state=1;
kbruil 0:86fe02a64f0e 281 vy = vy + ay;
kbruil 0:86fe02a64f0e 282 if (vy>3.0f){
kbruil 0:86fe02a64f0e 283 vy=3.0f;
kbruil 0:86fe02a64f0e 284 }
kbruil 0:86fe02a64f0e 285 ledr.write(0);
kbruil 0:86fe02a64f0e 286 }
kbruil 1:ffc1a2d6a824 287 */
kbruil 1:ffc1a2d6a824 288 if (switch1.read()){
kbruil 1:ffc1a2d6a824 289 end.x += spd;
kbruil 1:ffc1a2d6a824 290 }
kbruil 1:ffc1a2d6a824 291 if (switch2.read()){
kbruil 1:ffc1a2d6a824 292 end.x -= spd;
kbruil 1:ffc1a2d6a824 293 }
kbruil 1:ffc1a2d6a824 294 if (switch3.read()){
kbruil 1:ffc1a2d6a824 295 end.y += spd;
kbruil 1:ffc1a2d6a824 296 }
kbruil 1:ffc1a2d6a824 297 if (switch4.read()){
kbruil 1:ffc1a2d6a824 298 end.y -= spd;
kbruil 1:ffc1a2d6a824 299 }
kbruil 0:86fe02a64f0e 300 }
kbruil 0:86fe02a64f0e 301
kbruil 0:86fe02a64f0e 302 int main(){
kbruil 0:86fe02a64f0e 303 pc.baud(115200);
kbruil 0:86fe02a64f0e 304 robot_init();
kbruil 0:86fe02a64f0e 305 while(true){
kbruil 0:86fe02a64f0e 306 inputswitches();
kbruil 1:ffc1a2d6a824 307 robot_setposition(end.x,end.y);
kbruil 0:86fe02a64f0e 308 printf("Angle1\n\r");
kbruil 0:86fe02a64f0e 309 printf("%f\n\r", arm1.theta);
kbruil 0:86fe02a64f0e 310 printf("Angle2\n\r");
kbruil 0:86fe02a64f0e 311 printf("%f\n\r", arm2.theta);
kbruil 0:86fe02a64f0e 312 printf("x: %f | y: %f\n\r", end.x, end.y);
kbruil 1:ffc1a2d6a824 313 // printf("%f || %f", ARM1_ALIMLO, ARM1_ALIMHI);
kbruil 0:86fe02a64f0e 314
kbruil 0:86fe02a64f0e 315 // for(int i=0;i<30;i++)
kbruil 0:86fe02a64f0e 316 // {
kbruil 0:86fe02a64f0e 317 // printf("\n");
kbruil 0:86fe02a64f0e 318 // }
kbruil 0:86fe02a64f0e 319 /*
kbruil 0:86fe02a64f0e 320 printf("speed %f\n\r",vx);
kbruil 0:86fe02a64f0e 321 printf("end(x,y,theta) %f.2 : %f.2 : %f.2\n\r",end.x, end.y, arm2.theta);
kbruil 0:86fe02a64f0e 322 printf("arm2(x,y,theta) %f.2 : %f.2 : %f.2\n\r",arm2.x,arm2.y, arm1.theta);
kbruil 0:86fe02a64f0e 323 printf("end(vx, vy, theta) %f.2 : %f.2 : %f.2\n\r", end.vx, end.vy, arm2.omega);
kbruil 0:86fe02a64f0e 324 printf("arm2(vx, vy, theta) %f.2 : %f.2 : %f.2\n\r", arm2.vx, arm2.vy, arm1.omega);
kbruil 0:86fe02a64f0e 325 */
kbruil 0:86fe02a64f0e 326 wait(0.1f);
kbruil 0:86fe02a64f0e 327 }
kbruil 0:86fe02a64f0e 328 }