Kim Bruil / Mbed 2 deprecated r_robot

robot positie error test ding

Dependencies:   MODSERIAL mbed EMG_Input QEI biquadFilter

main.cpp@3:339b6f68b317, 2016-10-25 (annotated)

Committer:
kbruil
Date:
Tue Oct 25 12:36:58 2016 +0000
Revision:
3:339b6f68b317
Parent:
2:027d19cda97b
Child:
4:89f7d7f3a7ca
Bugs fixed, cleaning code

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 3:339b6f68b317 10 const double R_XTABLE_LEFT = -30.0; // Robot in the middle, table width=60
kbruil 3:339b6f68b317 11 const double R_XTABLE_RIGHT = 30.0;
kbruil 3:339b6f68b317 12 const double R_YTABLE_BOTTOM = -18.0;
kbruil 3:339b6f68b317 13 const double R_YTABLE_TOP = 82.0;
kbruil 3:339b6f68b317 14 const double R_ROBOT_LEFT = -8.0; // Robot limited from moving onto itself (20x20 base)
kbruil 3:339b6f68b317 15 const double R_ROBOT_RIGHT = 8.0;
kbruil 3:339b6f68b317 16 const double R_ROBOT_BOTTOM = -18.0;
kbruil 3:339b6f68b317 17 const double R_ROBOT_TOP = 2.0;
kbruil 1:ffc1a2d6a824 18
kbruil 3:339b6f68b317 19 const double ARM1_ALIMLO = (2*M_PI/360)*120;
kbruil 3:339b6f68b317 20 const double ARM1_ALIMHI = -(2*M_PI/360)*120;
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 2:027d19cda97b 24 float vx=0;
kbruil 2:027d19cda97b 25 float vy=0;
kbruil 0:86fe02a64f0e 26 // Struct r_link:
kbruil 0:86fe02a64f0e 27 // Defines a robot link type (arm or end effector).
kbruil 0:86fe02a64f0e 28 struct r_link {
kbruil 0:86fe02a64f0e 29 float length; // link length
kbruil 0:86fe02a64f0e 30 float x; // link x position
kbruil 0:86fe02a64f0e 31 float y; // link y position
kbruil 0:86fe02a64f0e 32 float vx; // link x speed
kbruil 0:86fe02a64f0e 33 float vy; // link y speed
kbruil 0:86fe02a64f0e 34 float ax; // link x accelleration
kbruil 0:86fe02a64f0e 35 float ay; // link y accelleration
kbruil 0:86fe02a64f0e 36 float theta; // link angle
kbruil 0:86fe02a64f0e 37 float omega; // link angular velocity
kbruil 0:86fe02a64f0e 38 float alpha; // link angular accelleration
kbruil 0:86fe02a64f0e 39 };
kbruil 0:86fe02a64f0e 40
kbruil 0:86fe02a64f0e 41 MODSERIAL pc(USBTX, USBRX);
kbruil 0:86fe02a64f0e 42 DigitalIn switch1(SW3);
kbruil 0:86fe02a64f0e 43 DigitalIn switch2(SW2);
kbruil 0:86fe02a64f0e 44 DigitalIn switch3(D7);
kbruil 0:86fe02a64f0e 45 DigitalIn switch4(D9);
kbruil 0:86fe02a64f0e 46 DigitalOut ledr(LED_RED);
kbruil 0:86fe02a64f0e 47 DigitalOut ledg(LED_GREEN);
kbruil 0:86fe02a64f0e 48
kbruil 0:86fe02a64f0e 49
kbruil 0:86fe02a64f0e 50 r_link arm1; // Robot upper arm
kbruil 0:86fe02a64f0e 51 r_link arm2; // Robot lower arm
kbruil 0:86fe02a64f0e 52 r_link end; // Robot end effector
kbruil 0:86fe02a64f0e 53
kbruil 3:339b6f68b317 54 /*
kbruil 3:339b6f68b317 55 ** =============================================================================
kbruil 3:339b6f68b317 56 ** robot_applySetpointLimits
kbruil 3:339b6f68b317 57 ** =============================================================================
kbruil 3:339b6f68b317 58 ** Function:
kbruil 3:339b6f68b317 59 ** Limits the cartesian coordinates to table, length of robot and prevents
kbruil 3:339b6f68b317 60 ** the robot arm crossing it's own base.
kbruil 3:339b6f68b317 61 ** Input:
kbruil 3:339b6f68b317 62 ** float x, float y: position in cartesian coordinates
kbruil 3:339b6f68b317 63 ** Output:
kbruil 3:339b6f68b317 64 ** float x, float y: limited position in cartesian coordinates
kbruil 3:339b6f68b317 65 ** =============================================================================
kbruil 3:339b6f68b317 66 */
kbruil 3:339b6f68b317 67 void robot_applySetpointLimits (float& x, float& y){
kbruil 0:86fe02a64f0e 68 float z;
kbruil 0:86fe02a64f0e 69 float angle;
kbruil 0:86fe02a64f0e 70
kbruil 0:86fe02a64f0e 71 // Limit x,y to table top size
kbruil 3:339b6f68b317 72 if (x<R_XTABLE_LEFT) { x=R_XTABLE_LEFT; vx=0;}
kbruil 3:339b6f68b317 73 else if (x>R_XTABLE_RIGHT) { x=R_XTABLE_RIGHT; vx=0;}
kbruil 3:339b6f68b317 74 if (y<R_YTABLE_BOTTOM) { y=R_YTABLE_BOTTOM; vy=0;}
kbruil 3:339b6f68b317 75 else if (y>R_YTABLE_TOP) { y=R_YTABLE_TOP; vy=0; }
kbruil 3:339b6f68b317 76
kbruil 3:339b6f68b317 77 // Limit x,y to maximum reach of robot arm
kbruil 0:86fe02a64f0e 78 z = sqrt(pow(x,2)+pow(y,2));
kbruil 0:86fe02a64f0e 79 angle = atan2(y,x);
kbruil 0:86fe02a64f0e 80 if (z>(arm1.length+arm2.length)){
kbruil 0:86fe02a64f0e 81 z = arm1.length+arm2.length;
kbruil 0:86fe02a64f0e 82 x = z*cos(angle);
kbruil 0:86fe02a64f0e 83 y = z*sin(angle);
kbruil 0:86fe02a64f0e 84 }
kbruil 3:339b6f68b317 85
kbruil 3:339b6f68b317 86 // Prevent x,y from entering robot body
kbruil 3:339b6f68b317 87 if (end.x > R_ROBOT_LEFT && end.x < R_ROBOT_RIGHT && end.y >= R_ROBOT_TOP && y < R_ROBOT_TOP) {
kbruil 3:339b6f68b317 88 y = R_ROBOT_TOP;
kbruil 3:339b6f68b317 89 }
kbruil 3:339b6f68b317 90 if (end.x <= R_ROBOT_LEFT && x > R_ROBOT_LEFT && y < R_ROBOT_TOP) {
kbruil 3:339b6f68b317 91 x = R_ROBOT_LEFT;
kbruil 3:339b6f68b317 92 }
kbruil 3:339b6f68b317 93 if (end.x >= R_ROBOT_RIGHT && x < R_ROBOT_RIGHT && y < R_ROBOT_TOP) {
kbruil 3:339b6f68b317 94 x = R_ROBOT_RIGHT;
kbruil 3:339b6f68b317 95 }
kbruil 0:86fe02a64f0e 96 }
kbruil 0:86fe02a64f0e 97
kbruil 3:339b6f68b317 98 /*
kbruil 3:339b6f68b317 99 ** =============================================================================
kbruil 3:339b6f68b317 100 ** robot_setposition
kbruil 3:339b6f68b317 101 ** =============================================================================
kbruil 3:339b6f68b317 102 ** Function:
kbruil 3:339b6f68b317 103 ** Transforms cartesian coordinates into rotadions of the joints of the robot's
kbruil 3:339b6f68b317 104 ** arms.
kbruil 3:339b6f68b317 105 ** Input:
kbruil 3:339b6f68b317 106 ** float x, float y: position in cartesian coordinates
kbruil 3:339b6f68b317 107 ** =============================================================================
kbruil 3:339b6f68b317 108 */
kbruil 3:339b6f68b317 109 void robot_setSetpoint (float x,float y){
kbruil 3:339b6f68b317 110 /*
kbruil 3:339b6f68b317 111 (x,y)
kbruil 3:339b6f68b317 112 |\
kbruil 3:339b6f68b317 113 | \
kbruil 3:339b6f68b317 114 |b2\
kbruil 3:339b6f68b317 115 z2 | \arm2.length
kbruil 3:339b6f68b317 116 | \
kbruil 3:339b6f68b317 117 z |_ _ _\
kbruil 3:339b6f68b317 118 | /
kbruil 3:339b6f68b317 119 | /
kbruil 3:339b6f68b317 120 z1 | /arm1.length
kbruil 3:339b6f68b317 121 |b1/
kbruil 3:339b6f68b317 122 | /
kbruil 3:339b6f68b317 123 |/
kbruil 0:86fe02a64f0e 124
kbruil 3:339b6f68b317 125 */
kbruil 3:339b6f68b317 126 float z; // length z of vector (x,y)
kbruil 3:339b6f68b317 127 float z1; // part z1
kbruil 3:339b6f68b317 128 float z2; // part z2
kbruil 3:339b6f68b317 129
kbruil 3:339b6f68b317 130 float angle; // angle of vector (x,y), !pointing up (x==0) the angle is defined as 0!
kbruil 3:339b6f68b317 131 float b1,b2; // angle b1 and b2 (see drawing)
kbruil 0:86fe02a64f0e 132
kbruil 3:339b6f68b317 133 float old; // 'old' helper variable used for calculating speeds
kbruil 3:339b6f68b317 134
kbruil 3:339b6f68b317 135
kbruil 3:339b6f68b317 136 robot_applySetpointLimits(x,y); // Apply limits
kbruil 1:ffc1a2d6a824 137
kbruil 3:339b6f68b317 138 z = sqrt(pow(x,2)+pow(y,2)); // Calculate (x,y) length
kbruil 3:339b6f68b317 139
kbruil 3:339b6f68b317 140 // Calculate (x,y) angle
kbruil 3:339b6f68b317 141 angle = atan2(y,x); // Calculate (x,y) angle using atan 2
kbruil 3:339b6f68b317 142 if (angle<0.5*M_PI && angle>0){ // Rotate result of atan2 90 degrees counter clockwise, so up pointing vector (x==0, y>0) has a zero degree rotation instead of 90 degrees.
kbruil 1:ffc1a2d6a824 143
kbruil 1:ffc1a2d6a824 144 angle = angle - 0.5*M_PI;
kbruil 0:86fe02a64f0e 145 }
kbruil 1:ffc1a2d6a824 146 else if (angle<M_PI && angle>0.5*M_PI){
kbruil 1:ffc1a2d6a824 147 angle = angle - 0.5*M_PI;
kbruil 0:86fe02a64f0e 148 }
kbruil 1:ffc1a2d6a824 149 else if (angle<0 && angle>-0.5*M_PI){
kbruil 1:ffc1a2d6a824 150 angle = angle - 0.5 * M_PI;
kbruil 1:ffc1a2d6a824 151 }
kbruil 1:ffc1a2d6a824 152 else{
kbruil 1:ffc1a2d6a824 153 angle = angle + 1.5*M_PI;
kbruil 1:ffc1a2d6a824 154 }
kbruil 3:339b6f68b317 155 if (y==0 && x<0) { angle = 0.5*M_PI; } // Special case correction: y=0, x < 0 Normally this returns PI, now it must return PI/2
kbruil 3:339b6f68b317 156 if (y==0 && x>0) { angle = -0.5*M_PI; } // Special case correction: y=0, x > 0 Normally this returns -PI, now it must return -PI/2
kbruil 2:027d19cda97b 157
kbruil 3:339b6f68b317 158 z1 = (pow(arm1.length,2)-pow(arm2.length,2)+pow(z,2))/(2*z); // Calculate z1
kbruil 3:339b6f68b317 159 z2 = z-z1; // Calculate z2
kbruil 3:339b6f68b317 160 // Roundoff errors sometimes make z1 and z2 slightly higher than the arm lengths, this causes erratic behaviour in the acos function,
kbruil 3:339b6f68b317 161 // so it is countered here by an extra check and fix.
kbruil 1:ffc1a2d6a824 162 if (z1>arm1.length) {z1 = arm1.length;}
kbruil 1:ffc1a2d6a824 163 else if (z1<-arm1.length) { z1 = -arm1.length;}
kbruil 1:ffc1a2d6a824 164 if (z2>arm2.length) {z2 = arm2.length;}
kbruil 1:ffc1a2d6a824 165 else if (z2<-arm2.length) { z2 = -arm2.length;}
kbruil 0:86fe02a64f0e 166
kbruil 3:339b6f68b317 167 b1 = acos(z1/arm1.length); // Calculate angle b1
kbruil 3:339b6f68b317 168 b2 = acos(z2/arm2.length); // Calculate angle b2
kbruil 0:86fe02a64f0e 169
kbruil 3:339b6f68b317 170 // Arm 1 angle and rotational speed
kbruil 3:339b6f68b317 171 old = arm1.theta; // Preserve old angle arm 1
kbruil 3:339b6f68b317 172 arm1.theta = angle - b1; // Calculate new angle arm 1
kbruil 0:86fe02a64f0e 173
kbruil 3:339b6f68b317 174 if (arm1.theta>ARM1_ALIMLO){ // Limit angle arm1
kbruil 3:339b6f68b317 175 arm1.theta = ARM1_ALIMLO;
kbruil 0:86fe02a64f0e 176 }
kbruil 0:86fe02a64f0e 177 else if (arm1.theta<ARM1_ALIMHI){
kbruil 3:339b6f68b317 178 arm1.theta = ARM1_ALIMHI;
kbruil 0:86fe02a64f0e 179 }
kbruil 1:ffc1a2d6a824 180
kbruil 3:339b6f68b317 181 arm1.omega = arm1.theta - old; // Calculate rotational speed arm 1
kbruil 3:339b6f68b317 182
kbruil 3:339b6f68b317 183 // Arm 2 angle and rotational speed
kbruil 3:339b6f68b317 184 old = arm2.theta; // Preserve old angle arm 2
kbruil 3:339b6f68b317 185 arm2.theta = b1 + b2; // Calculate new angle arm 2
kbruil 3:339b6f68b317 186 if (arm2.theta>ARM2_ALIMHI){ // Limit angle arm 2
kbruil 0:86fe02a64f0e 187 arm2.theta = ARM2_ALIMHI;
kbruil 1:ffc1a2d6a824 188 } else if (arm2.theta < 0) {
kbruil 1:ffc1a2d6a824 189 arm2.theta = 0;
kbruil 0:86fe02a64f0e 190 }
kbruil 1:ffc1a2d6a824 191
kbruil 3:339b6f68b317 192 arm2.omega = arm2.theta - old; // Calculate rotational speed arm 2
kbruil 0:86fe02a64f0e 193
kbruil 3:339b6f68b317 194 // Arm 2 position
kbruil 3:339b6f68b317 195 old = arm2.x; // Preserve old x position arm 2
kbruil 3:339b6f68b317 196 arm2.x = arm1.length*-sin(arm1.theta); // Calculate new x position arm 2
kbruil 3:339b6f68b317 197 arm2.vx = arm2.x - old; // Calculate new x speed arm 2
kbruil 3:339b6f68b317 198 old = arm2.y; // Preserve old y position arm 2
kbruil 3:339b6f68b317 199 arm2.y = arm1.length*cos(arm1.theta); // Calculate new y position arm 2
kbruil 3:339b6f68b317 200 arm2.vy = arm2.y - old; // Calculate new y speed arm 2
kbruil 3:339b6f68b317 201
kbruil 3:339b6f68b317 202 old = end.x; // Preserve old x position end effector
kbruil 3:339b6f68b317 203 end.x = arm2.length*(-sin(arm1.theta+arm2.theta)) + arm2.x; // Calculate new x position end effector
kbruil 3:339b6f68b317 204 end.vx = end.x - old; // Calculate new x speed end effector
kbruil 3:339b6f68b317 205 old = end.y; // Preserve old y position end effector
kbruil 3:339b6f68b317 206 end.y = arm2.length*(cos(arm1.theta+arm2.theta)) + arm2.y; // Calculate new y position end effector
kbruil 3:339b6f68b317 207 end.vy = old - end.y; // Calculate new y speed end effector
kbruil 1:ffc1a2d6a824 208
kbruil 0:86fe02a64f0e 209 }
kbruil 0:86fe02a64f0e 210
kbruil 0:86fe02a64f0e 211 // robot_init
kbruil 0:86fe02a64f0e 212 // Initialise robot
kbruil 0:86fe02a64f0e 213 void robot_init() {
kbruil 0:86fe02a64f0e 214 // Init arm1 (upper arm) first link
kbruil 0:86fe02a64f0e 215 arm1.length = 28.0f;
kbruil 0:86fe02a64f0e 216 arm1.x = 0; arm1.y = 0;
kbruil 0:86fe02a64f0e 217 arm1.vx = 0; arm1.vy = 0;
kbruil 0:86fe02a64f0e 218 arm1.ax = 0; arm1.ay = 0;
kbruil 1:ffc1a2d6a824 219 arm1.theta = 0;
kbruil 0:86fe02a64f0e 220 arm1.omega = 0;
kbruil 0:86fe02a64f0e 221 arm1.alpha = 0;
kbruil 0:86fe02a64f0e 222
kbruil 0:86fe02a64f0e 223 // Init arm2 (lower arm) second link
kbruil 0:86fe02a64f0e 224 arm2.length = 35.0f;
kbruil 0:86fe02a64f0e 225 arm2.x = 0; arm2.y = arm1.length;
kbruil 0:86fe02a64f0e 226 arm2.vx = 0; arm2.vy = 0;
kbruil 0:86fe02a64f0e 227 arm2.ax = 0; arm2.ay = 0;
kbruil 1:ffc1a2d6a824 228 arm2.theta = 0;
kbruil 0:86fe02a64f0e 229 arm2.omega = 0;
kbruil 0:86fe02a64f0e 230 arm2.alpha = 0;
kbruil 0:86fe02a64f0e 231
kbruil 0:86fe02a64f0e 232 // Init end (end effector) third/last link
kbruil 0:86fe02a64f0e 233 end.length = 30.0f;
kbruil 0:86fe02a64f0e 234 end.x = 0; end.y = arm1.length+arm2.length;
kbruil 0:86fe02a64f0e 235 end.vx = 0; end.vy = 0;
kbruil 0:86fe02a64f0e 236 end.ax = 0; end.ay = 0;
kbruil 0:86fe02a64f0e 237 end.theta = 0;
kbruil 0:86fe02a64f0e 238 end.omega = 0;
kbruil 0:86fe02a64f0e 239 end.alpha = 0;
kbruil 0:86fe02a64f0e 240 }
kbruil 0:86fe02a64f0e 241
kbruil 0:86fe02a64f0e 242 int lr_state=0;
kbruil 0:86fe02a64f0e 243 int ud_state=0;
kbruil 0:86fe02a64f0e 244 int sw2_state=0;
kbruil 0:86fe02a64f0e 245 float ax=0.1;
kbruil 0:86fe02a64f0e 246 float ay=0.1;
kbruil 2:027d19cda97b 247
kbruil 1:ffc1a2d6a824 248 float spd=1.0;
kbruil 0:86fe02a64f0e 249
kbruil 0:86fe02a64f0e 250 void inputswitches()
kbruil 0:86fe02a64f0e 251 {
kbruil 2:027d19cda97b 252 if(switch1.read())
kbruil 0:86fe02a64f0e 253 {
kbruil 0:86fe02a64f0e 254 if (lr_state==0){
kbruil 0:86fe02a64f0e 255 vx=0;
kbruil 0:86fe02a64f0e 256 ledr.write(1);
kbruil 0:86fe02a64f0e 257 }
kbruil 0:86fe02a64f0e 258 }
kbruil 0:86fe02a64f0e 259 else
kbruil 0:86fe02a64f0e 260 {
kbruil 0:86fe02a64f0e 261 lr_state=0;
kbruil 0:86fe02a64f0e 262 vx = vx - ax;
kbruil 0:86fe02a64f0e 263 if (vx<-3.0f){
kbruil 0:86fe02a64f0e 264 vx=-3.0f;
kbruil 0:86fe02a64f0e 265 }
kbruil 0:86fe02a64f0e 266 ledr.write(0);
kbruil 0:86fe02a64f0e 267 }
kbruil 0:86fe02a64f0e 268
kbruil 0:86fe02a64f0e 269 if(switch2.read())
kbruil 0:86fe02a64f0e 270 {
kbruil 0:86fe02a64f0e 271 if(lr_state==1){
kbruil 0:86fe02a64f0e 272 vx=0;
kbruil 0:86fe02a64f0e 273 ledr.write(1);
kbruil 0:86fe02a64f0e 274 }
kbruil 0:86fe02a64f0e 275 }
kbruil 0:86fe02a64f0e 276 else
kbruil 0:86fe02a64f0e 277 {
kbruil 0:86fe02a64f0e 278 lr_state=1;
kbruil 0:86fe02a64f0e 279 vx = vx + ax;
kbruil 0:86fe02a64f0e 280 if (vx>3.0f){
kbruil 0:86fe02a64f0e 281 vx=3.0f;
kbruil 0:86fe02a64f0e 282 }
kbruil 0:86fe02a64f0e 283 ledr.write(0);
kbruil 0:86fe02a64f0e 284 }
kbruil 0:86fe02a64f0e 285 if(switch3.read())
kbruil 0:86fe02a64f0e 286 {
kbruil 0:86fe02a64f0e 287 if (ud_state==0){
kbruil 0:86fe02a64f0e 288 vy=0;
kbruil 0:86fe02a64f0e 289 ledr.write(1);
kbruil 0:86fe02a64f0e 290 }
kbruil 0:86fe02a64f0e 291 }
kbruil 0:86fe02a64f0e 292 else
kbruil 0:86fe02a64f0e 293 {
kbruil 0:86fe02a64f0e 294 ud_state=0;
kbruil 0:86fe02a64f0e 295 vy = vy - ay;
kbruil 0:86fe02a64f0e 296 if (vy<-3.0f){
kbruil 0:86fe02a64f0e 297 vy=-3.0f;
kbruil 0:86fe02a64f0e 298 }
kbruil 0:86fe02a64f0e 299 ledr.write(0);
kbruil 0:86fe02a64f0e 300 }
kbruil 0:86fe02a64f0e 301
kbruil 0:86fe02a64f0e 302 if(switch4.read())
kbruil 0:86fe02a64f0e 303 {
kbruil 0:86fe02a64f0e 304 if(ud_state==1){
kbruil 0:86fe02a64f0e 305 vy=0;
kbruil 0:86fe02a64f0e 306 ledr.write(1);
kbruil 0:86fe02a64f0e 307 }
kbruil 0:86fe02a64f0e 308 }
kbruil 0:86fe02a64f0e 309 else
kbruil 0:86fe02a64f0e 310 {
kbruil 0:86fe02a64f0e 311 ud_state=1;
kbruil 0:86fe02a64f0e 312 vy = vy + ay;
kbruil 0:86fe02a64f0e 313 if (vy>3.0f){
kbruil 0:86fe02a64f0e 314 vy=3.0f;
kbruil 0:86fe02a64f0e 315 }
kbruil 0:86fe02a64f0e 316 ledr.write(0);
kbruil 0:86fe02a64f0e 317 }
kbruil 2:027d19cda97b 318 /*
kbruil 1:ffc1a2d6a824 319 if (switch1.read()){
kbruil 1:ffc1a2d6a824 320 end.x += spd;
kbruil 1:ffc1a2d6a824 321 }
kbruil 1:ffc1a2d6a824 322 if (switch2.read()){
kbruil 1:ffc1a2d6a824 323 end.x -= spd;
kbruil 1:ffc1a2d6a824 324 }
kbruil 1:ffc1a2d6a824 325 if (switch3.read()){
kbruil 1:ffc1a2d6a824 326 end.y += spd;
kbruil 1:ffc1a2d6a824 327 }
kbruil 1:ffc1a2d6a824 328 if (switch4.read()){
kbruil 1:ffc1a2d6a824 329 end.y -= spd;
kbruil 1:ffc1a2d6a824 330 }
kbruil 2:027d19cda97b 331 */
kbruil 0:86fe02a64f0e 332 }
kbruil 0:86fe02a64f0e 333
kbruil 0:86fe02a64f0e 334 int main(){
kbruil 0:86fe02a64f0e 335 pc.baud(115200);
kbruil 0:86fe02a64f0e 336 robot_init();
kbruil 0:86fe02a64f0e 337 while(true){
kbruil 0:86fe02a64f0e 338 inputswitches();
kbruil 3:339b6f68b317 339 robot_setSetpoint(end.x+vx,end.y+vy);
kbruil 0:86fe02a64f0e 340 printf("Angle1\n\r");
kbruil 0:86fe02a64f0e 341 printf("%f\n\r", arm1.theta);
kbruil 0:86fe02a64f0e 342 printf("Angle2\n\r");
kbruil 0:86fe02a64f0e 343 printf("%f\n\r", arm2.theta);
kbruil 0:86fe02a64f0e 344 printf("x: %f | y: %f\n\r", end.x, end.y);
kbruil 1:ffc1a2d6a824 345 // printf("%f || %f", ARM1_ALIMLO, ARM1_ALIMHI);
kbruil 0:86fe02a64f0e 346
kbruil 0:86fe02a64f0e 347 // for(int i=0;i<30;i++)
kbruil 0:86fe02a64f0e 348 // {
kbruil 0:86fe02a64f0e 349 // printf("\n");
kbruil 0:86fe02a64f0e 350 // }
kbruil 0:86fe02a64f0e 351 /*
kbruil 0:86fe02a64f0e 352 printf("speed %f\n\r",vx);
kbruil 0:86fe02a64f0e 353 printf("end(x,y,theta) %f.2 : %f.2 : %f.2\n\r",end.x, end.y, arm2.theta);
kbruil 0:86fe02a64f0e 354 printf("arm2(x,y,theta) %f.2 : %f.2 : %f.2\n\r",arm2.x,arm2.y, arm1.theta);
kbruil 0:86fe02a64f0e 355 printf("end(vx, vy, theta) %f.2 : %f.2 : %f.2\n\r", end.vx, end.vy, arm2.omega);
kbruil 0:86fe02a64f0e 356 printf("arm2(vx, vy, theta) %f.2 : %f.2 : %f.2\n\r", arm2.vx, arm2.vy, arm1.omega);
kbruil 0:86fe02a64f0e 357 */
kbruil 0:86fe02a64f0e 358 wait(0.1f);
kbruil 0:86fe02a64f0e 359 }
kbruil 0:86fe02a64f0e 360 }