Kim Bruil / Mbed 2 deprecated r_robot

robot positie error test ding

Dependencies:   MODSERIAL mbed EMG_Input QEI biquadFilter

main.cpp@0:86fe02a64f0e, 2016-10-24 (annotated)

Committer:
kbruil
Date:
Mon Oct 24 08:26:06 2016 +0000
Revision:
0:86fe02a64f0e
Child:
1:ffc1a2d6a824
robot control error;

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