BE@R lab / Mbed 2 deprecated dog_V3_3_testmotor

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   Communication_Robot QEI iSerial mbed motion_control

Fork of dog_V3_2_testmotor by BE@R lab

main.cpp@12:2564eac22e0a, 2015-07-20 (annotated)

Committer:
soulx
Date:
Mon Jul 20 09:15:40 2015 +0000
Revision:
12:2564eac22e0a
Parent:
11:336dd293daa1
Child:
13:218c22a620cc
add function test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
soulx 0:2433ddae2772 1 #include "mbed.h"
soulx 0:2433ddae2772 2 #include "pin_config.h"
soulx 0:2433ddae2772 3
soulx 6:8d80c84e0c09 4 #include "communication.h"
soulx 6:8d80c84e0c09 5 #include "protocol.h"
soulx 11:336dd293daa1 6 #include "iSerial.h"
soulx 6:8d80c84e0c09 7
soulx 10:53cb691e22bf 8 #include "motor_relay.h"
soulx 10:53cb691e22bf 9 #include "motion_control.h"
soulx 10:53cb691e22bf 10
soulx 6:8d80c84e0c09 11 //set frequancy unit in Hz
soulx 6:8d80c84e0c09 12 #define F_UPDATE 10.0f
soulx 6:8d80c84e0c09 13 #define TIMER_UPDATE 1.0f/F_UPDATE
soulx 6:8d80c84e0c09 14
soulx 6:8d80c84e0c09 15 //counter not receive from station
soulx 6:8d80c84e0c09 16 #define TIMEOUT_RESPONE_COMMAND 5
soulx 10:53cb691e22bf 17
soulx 10:53cb691e22bf 18 MOTION_CONTROL leg_left_upper(INA_L_U,INB_L_U,LIMIT_LU_U,LIMIT_LU_D,VR_LU);
soulx 10:53cb691e22bf 19 MOTION_CONTROL leg_left_lower(INA_L_L,INB_L_L,LIMIT_LL_U,LIMIT_LL_D,VR_LL);
soulx 10:53cb691e22bf 20 MOTION_CONTROL leg_right_upper(INA_R_U,INB_R_U,LIMIT_RU_U,LIMIT_RU_D,VR_RU);
soulx 10:53cb691e22bf 21 MOTION_CONTROL leg_right_lower(INA_R_L,INB_R_L,LIMIT_RL_U,LIMIT_RL_D,VR_RL);
soulx 10:53cb691e22bf 22
soulx 0:2433ddae2772 23 DigitalOut myled(LED1);
soulx 0:2433ddae2772 24 DigitalIn mybutton(USER_BUTTON);
soulx 0:2433ddae2772 25
soulx 6:8d80c84e0c09 26 //communication config
soulx 6:8d80c84e0c09 27 //serial for debug
soulx 11:336dd293daa1 28 iSerial pc(USBTX, USBRX);
soulx 6:8d80c84e0c09 29 //serial for xbee
soulx 6:8d80c84e0c09 30 COMMUNICATION pan_a(TX_XBEE, RX_XBEE,115200,1000,1000); // tx, rx
soulx 0:2433ddae2772 31
soulx 11:336dd293daa1 32 //Fuction prototye
soulx 11:336dd293daa1 33 void getCommand();
soulx 11:336dd293daa1 34 // init function
soulx 11:336dd293daa1 35 void calibration();
soulx 11:336dd293daa1 36 void test_position();
soulx 12:2564eac22e0a 37 void test_status();
soulx 12:2564eac22e0a 38 void test_limit();
soulx 12:2564eac22e0a 39 void test_motor();
soulx 0:2433ddae2772 40
soulx 12:2564eac22e0a 41 void routine();
soulx 12:2564eac22e0a 42
soulx 12:2564eac22e0a 43 void management(ANDANTE_PROTOCOL_PACKET *packet);
soulx 0:2433ddae2772 44
soulx 11:336dd293daa1 45 void copy_data(ANDANTE_PROTOCOL_PACKET *scr, ANDANTE_PROTOCOL_PACKET *dst);
soulx 0:2433ddae2772 46
soulx 6:8d80c84e0c09 47
soulx 6:8d80c84e0c09 48 //set foreground
soulx 6:8d80c84e0c09 49 Ticker Update_command;
soulx 6:8d80c84e0c09 50
soulx 11:336dd293daa1 51 Timer t;
soulx 11:336dd293daa1 52
soulx 12:2564eac22e0a 53 //struct
soulx 12:2564eac22e0a 54 struct PARAM_WRITE {
soulx 12:2564eac22e0a 55 uint16_t left_up;
soulx 12:2564eac22e0a 56 uint16_t left_down;
soulx 12:2564eac22e0a 57 uint16_t right_up;
soulx 12:2564eac22e0a 58 uint16_t right_down;
soulx 12:2564eac22e0a 59 };
soulx 12:2564eac22e0a 60
soulx 11:336dd293daa1 61 //variable
soulx 12:2564eac22e0a 62 //volatile ANDANTE_PROTOCOL_PACKET *param;
soulx 11:336dd293daa1 63 volatile uint8_t status=0;
soulx 12:2564eac22e0a 64 //volatile PARAM_WRITE buff;
soulx 12:2564eac22e0a 65 PARAM_WRITE buff;
soulx 6:8d80c84e0c09 66
soulx 0:2433ddae2772 67 //Main function
soulx 0:2433ddae2772 68 int main()
soulx 0:2433ddae2772 69 {
soulx 12:2564eac22e0a 70 //int state=0;
soulx 12:2564eac22e0a 71
soulx 11:336dd293daa1 72 pc.baud(115200);
soulx 11:336dd293daa1 73 pc.printf("Welcome to DOGWHEELSCHAIR\n");
soulx 12:2564eac22e0a 74
soulx 12:2564eac22e0a 75 buff.left_up = 0;
soulx 12:2564eac22e0a 76 buff.right_up =0;
soulx 12:2564eac22e0a 77 buff.left_down = 64;
soulx 12:2564eac22e0a 78 buff.right_down = 64;
soulx 12:2564eac22e0a 79
soulx 11:336dd293daa1 80 if(mybutton == 0) {
soulx 11:336dd293daa1 81 calibration();
soulx 11:336dd293daa1 82 } else {
soulx 11:336dd293daa1 83 pc.printf("Lock position Min-Max...");
soulx 11:336dd293daa1 84 leg_left_upper.SetMaxPosition(56244);
soulx 11:336dd293daa1 85 leg_left_upper.SetMinPosition(20806);
soulx 11:336dd293daa1 86
soulx 11:336dd293daa1 87 leg_left_lower.SetMaxPosition(50996);
soulx 11:336dd293daa1 88 leg_left_lower.SetMinPosition(5371);
soulx 11:336dd293daa1 89
soulx 11:336dd293daa1 90 leg_right_upper.SetMaxPosition(38985);
soulx 11:336dd293daa1 91 leg_right_upper.SetMinPosition(8545);
soulx 11:336dd293daa1 92
soulx 11:336dd293daa1 93 leg_right_lower.SetMaxPosition(38027);
soulx 11:336dd293daa1 94 leg_right_lower.SetMinPosition(40);
soulx 11:336dd293daa1 95 pc.printf("pass\n");
soulx 11:336dd293daa1 96 }
soulx 11:336dd293daa1 97
soulx 11:336dd293daa1 98 Update_command.attach(&getCommand,TIMER_UPDATE);
soulx 11:336dd293daa1 99
soulx 12:2564eac22e0a 100 test_motor();
soulx 12:2564eac22e0a 101 test_limit();
soulx 12:2564eac22e0a 102 test_status();
soulx 12:2564eac22e0a 103 routine();
soulx 11:336dd293daa1 104 }
soulx 11:336dd293daa1 105
soulx 11:336dd293daa1 106 void getCommand()
soulx 11:336dd293daa1 107 {
soulx 11:336dd293daa1 108 static uint8_t count =0;
soulx 11:336dd293daa1 109
soulx 11:336dd293daa1 110 ANDANTE_PROTOCOL_PACKET packet;
soulx 11:336dd293daa1 111
soulx 11:336dd293daa1 112 uint8_t status = pan_a.receiveCommunicatePacket(&packet);
soulx 11:336dd293daa1 113 myled=0;
soulx 11:336dd293daa1 114
soulx 11:336dd293daa1 115 if(status == ANDANTE_ERRBIT_NONE) {
soulx 11:336dd293daa1 116 if(count >2 && count <10) {
soulx 11:336dd293daa1 117 count--;
soulx 11:336dd293daa1 118 } else {
soulx 11:336dd293daa1 119 count=0;
soulx 5:f07cbb5a86c3 120 }
soulx 11:336dd293daa1 121
soulx 11:336dd293daa1 122
soulx 12:2564eac22e0a 123 //pan_a.sendCommunicatePacket(&packet);
soulx 11:336dd293daa1 124
soulx 11:336dd293daa1 125 //update command
soulx 11:336dd293daa1 126 //setControl(&packet);
soulx 11:336dd293daa1 127
soulx 12:2564eac22e0a 128
soulx 11:336dd293daa1 129 } else if(status == ANDANTE_ERRBIT_READ_TIMEOUT) {
soulx 11:336dd293daa1 130 count++;
soulx 11:336dd293daa1 131 }
soulx 5:f07cbb5a86c3 132
soulx 11:336dd293daa1 133 if(count >= TIMEOUT_RESPONE_COMMAND) {
soulx 11:336dd293daa1 134 //stop robot
soulx 11:336dd293daa1 135 count++;
soulx 11:336dd293daa1 136 myled=1;
soulx 11:336dd293daa1 137 // setSpeedControl(0.0);
soulx 11:336dd293daa1 138 }
soulx 11:336dd293daa1 139 }
soulx 0:2433ddae2772 140
soulx 11:336dd293daa1 141 void calibration()
soulx 11:336dd293daa1 142 {
soulx 11:336dd293daa1 143 //calibration
soulx 11:336dd293daa1 144 pc.printf("Calibration [START]\n");
soulx 11:336dd293daa1 145 leg_left_upper.calibration();
soulx 11:336dd293daa1 146 pc.printf("Left_UPPER\n");
soulx 11:336dd293daa1 147 pc.printf("max_position = %d\n",leg_left_upper.GetMaxPosition());
soulx 11:336dd293daa1 148 pc.printf("min_position = %d\n",leg_left_upper.GetMinPosition());
soulx 11:336dd293daa1 149 leg_left_lower.calibration();
soulx 11:336dd293daa1 150 pc.printf("Left_Lower\n");
soulx 11:336dd293daa1 151 pc.printf("max_position = %d\n",leg_left_lower.GetMaxPosition());
soulx 11:336dd293daa1 152 pc.printf("min_position = %d\n",leg_left_lower.GetMinPosition());
soulx 11:336dd293daa1 153 leg_right_upper.calibration();
soulx 11:336dd293daa1 154 pc.printf("right_UPPER\n");
soulx 11:336dd293daa1 155 pc.printf("max_position = %d\n",leg_right_upper.GetMaxPosition());
soulx 11:336dd293daa1 156 pc.printf("min_position = %d\n",leg_right_upper.GetMinPosition());
soulx 11:336dd293daa1 157 leg_right_lower.calibration();
soulx 11:336dd293daa1 158 pc.printf("right_Lower\n");
soulx 11:336dd293daa1 159 pc.printf("max_position = %d\n",leg_right_lower.GetMaxPosition());
soulx 11:336dd293daa1 160 pc.printf("min_position = %d\n",leg_right_lower.GetMinPosition());
soulx 0:2433ddae2772 161
soulx 11:336dd293daa1 162 pc.printf("Calibration [FINISH]\n");
soulx 11:336dd293daa1 163 pc.printf("RUN mode [START]\n");
soulx 11:336dd293daa1 164 wait(1);
soulx 11:336dd293daa1 165 }
soulx 11:336dd293daa1 166
soulx 11:336dd293daa1 167 void test_position()
soulx 11:336dd293daa1 168 {
soulx 11:336dd293daa1 169 uint8_t state;
soulx 11:336dd293daa1 170 do {
soulx 10:53cb691e22bf 171 /*
soulx 10:53cb691e22bf 172 state=0;
soulx 10:53cb691e22bf 173 //leg_left_upper.position_control(500);
soulx 10:53cb691e22bf 174 if(leg_left_lower.position_control(500) ==2)
soulx 10:53cb691e22bf 175 state++;
soulx 10:53cb691e22bf 176 if(leg_right_upper.position_control(500) == 2)
soulx 10:53cb691e22bf 177 state++;
soulx 10:53cb691e22bf 178 if(leg_right_lower.position_control(500) == 2)
soulx 10:53cb691e22bf 179 state++;
soulx 10:53cb691e22bf 180 */
soulx 10:53cb691e22bf 181 state = leg_left_upper.position_control(32);
soulx 10:53cb691e22bf 182 pc.printf("state_lu %d\n",state);
soulx 10:53cb691e22bf 183 state = leg_left_lower.position_control(32);
soulx 10:53cb691e22bf 184 pc.printf("state_ll %d\n",state);
soulx 10:53cb691e22bf 185 state = leg_right_upper.position_control(32);
soulx 10:53cb691e22bf 186 pc.printf("state_ru %d\n",state);
soulx 11:336dd293daa1 187 state = leg_right_lower.position_control(32);
soulx 10:53cb691e22bf 188 pc.printf("state_rl %d\n",state);
soulx 10:53cb691e22bf 189 state=0;
soulx 11:336dd293daa1 190 } while(state != 2);
soulx 10:53cb691e22bf 191 pc.printf("[Finish test]\n");
soulx 12:2564eac22e0a 192 }
soulx 12:2564eac22e0a 193
soulx 12:2564eac22e0a 194 void test_status()
soulx 12:2564eac22e0a 195 {
soulx 12:2564eac22e0a 196 uint16_t state=0;
soulx 12:2564eac22e0a 197 t.start();
soulx 12:2564eac22e0a 198 while(1) {
soulx 12:2564eac22e0a 199
soulx 12:2564eac22e0a 200
soulx 12:2564eac22e0a 201 if(t.read() > 10.0f) {
soulx 12:2564eac22e0a 202 t.reset();
soulx 12:2564eac22e0a 203 if(status >3) {
soulx 12:2564eac22e0a 204 status =0;
soulx 12:2564eac22e0a 205 } else {
soulx 12:2564eac22e0a 206 status++;
soulx 12:2564eac22e0a 207 }
soulx 12:2564eac22e0a 208 }
soulx 12:2564eac22e0a 209
soulx 12:2564eac22e0a 210 if(status == 0) {
soulx 12:2564eac22e0a 211 state =0;
soulx 12:2564eac22e0a 212 // sleep
soulx 12:2564eac22e0a 213 state += leg_left_upper.position_control(0);
soulx 12:2564eac22e0a 214 state += leg_right_upper.position_control(0);
soulx 12:2564eac22e0a 215 state += leg_left_lower.position_control(64);
soulx 12:2564eac22e0a 216 state += leg_right_lower.position_control(64);
soulx 12:2564eac22e0a 217 if(state == 8) {
soulx 12:2564eac22e0a 218 myled=1;
soulx 12:2564eac22e0a 219 } else {
soulx 12:2564eac22e0a 220 myled=0;
soulx 12:2564eac22e0a 221 }
soulx 12:2564eac22e0a 222 } else if(status == 1 || status ==3) {
soulx 12:2564eac22e0a 223 state =0;
soulx 12:2564eac22e0a 224 // sit
soulx 12:2564eac22e0a 225 state += leg_left_upper.position_control(64);
soulx 12:2564eac22e0a 226 state += leg_right_upper.position_control(64);
soulx 12:2564eac22e0a 227 state += leg_left_lower.position_control(64);
soulx 12:2564eac22e0a 228 state += leg_right_lower.position_control(64);
soulx 12:2564eac22e0a 229 if(state == 8) {
soulx 12:2564eac22e0a 230 myled=1;
soulx 12:2564eac22e0a 231 } else {
soulx 12:2564eac22e0a 232 myled=0;
soulx 12:2564eac22e0a 233 }
soulx 12:2564eac22e0a 234 } else if(status == 2) {
soulx 12:2564eac22e0a 235 state =0;
soulx 12:2564eac22e0a 236 // stand
soulx 12:2564eac22e0a 237 state += leg_left_upper.position_control(64);
soulx 12:2564eac22e0a 238 state += leg_right_upper.position_control(64);
soulx 12:2564eac22e0a 239 state += leg_left_lower.position_control(0);
soulx 12:2564eac22e0a 240 state += leg_right_lower.position_control(0);
soulx 12:2564eac22e0a 241 if(state == 8) {
soulx 12:2564eac22e0a 242 myled=1;
soulx 12:2564eac22e0a 243 } else {
soulx 12:2564eac22e0a 244 myled=0;
soulx 12:2564eac22e0a 245 }
soulx 12:2564eac22e0a 246 }
soulx 12:2564eac22e0a 247 }
soulx 12:2564eac22e0a 248 //t.stop();
soulx 12:2564eac22e0a 249 }
soulx 12:2564eac22e0a 250
soulx 12:2564eac22e0a 251 void management(ANDANTE_PROTOCOL_PACKET *packet)
soulx 12:2564eac22e0a 252 {
soulx 12:2564eac22e0a 253 switch(packet->instructionErrorId) {
soulx 12:2564eac22e0a 254 case 0x01:
soulx 12:2564eac22e0a 255 buff.left_up = Utilities::ConvertUInt8ArrayToInt16(packet->parameter);
soulx 12:2564eac22e0a 256 break;
soulx 12:2564eac22e0a 257 case 0x02:
soulx 12:2564eac22e0a 258 buff.left_down = Utilities::ConvertUInt8ArrayToInt16(packet->parameter);
soulx 12:2564eac22e0a 259 break;
soulx 12:2564eac22e0a 260 case 0x03:
soulx 12:2564eac22e0a 261 buff.right_up = Utilities::ConvertUInt8ArrayToInt16(packet->parameter);
soulx 12:2564eac22e0a 262 break;
soulx 12:2564eac22e0a 263 case 0x04:
soulx 12:2564eac22e0a 264 buff.right_down = Utilities::ConvertUInt8ArrayToInt16(packet->parameter);
soulx 12:2564eac22e0a 265 break;
soulx 12:2564eac22e0a 266 }
soulx 12:2564eac22e0a 267 }
soulx 12:2564eac22e0a 268
soulx 12:2564eac22e0a 269 void routine()
soulx 12:2564eac22e0a 270 {
soulx 12:2564eac22e0a 271 uint8_t state=0;
soulx 12:2564eac22e0a 272 // PARAM_WRITE pos = buff;
soulx 12:2564eac22e0a 273 while(1) {
soulx 12:2564eac22e0a 274 state =0;
soulx 12:2564eac22e0a 275 // sit
soulx 12:2564eac22e0a 276 state += leg_left_upper.position_control(buff.left_up);
soulx 12:2564eac22e0a 277 state += leg_right_upper.position_control(buff.right_up);
soulx 12:2564eac22e0a 278 state += leg_left_lower.position_control(buff.left_down);
soulx 12:2564eac22e0a 279 state += leg_right_lower.position_control(buff.right_down);
soulx 12:2564eac22e0a 280 if(state == 8) {
soulx 12:2564eac22e0a 281 myled=1;
soulx 12:2564eac22e0a 282 } else {
soulx 12:2564eac22e0a 283 myled=0;
soulx 12:2564eac22e0a 284 }
soulx 12:2564eac22e0a 285 }
soulx 12:2564eac22e0a 286 }
soulx 12:2564eac22e0a 287
soulx 12:2564eac22e0a 288 void test_limit()
soulx 12:2564eac22e0a 289 {
soulx 12:2564eac22e0a 290 pc.printf("TEST LIMIT ALL\n");
soulx 12:2564eac22e0a 291 while(1) {
soulx 12:2564eac22e0a 292 pc.printf("leftUP_limit_up = ");
soulx 12:2564eac22e0a 293 if(leg_left_upper.GetLimitUp() == 0)
soulx 12:2564eac22e0a 294 pc.printf("shot\n");
soulx 12:2564eac22e0a 295 else
soulx 12:2564eac22e0a 296 pc.printf("open\n");
soulx 12:2564eac22e0a 297
soulx 12:2564eac22e0a 298 pc.printf("leftUP_limit_down = ");
soulx 12:2564eac22e0a 299 if(leg_left_upper.GetLimitDown() == 0)
soulx 12:2564eac22e0a 300 pc.printf("shot\n");
soulx 12:2564eac22e0a 301 else
soulx 12:2564eac22e0a 302 pc.printf("open\n");
soulx 12:2564eac22e0a 303
soulx 12:2564eac22e0a 304 pc.printf("leftLOW_limit_up = ");
soulx 12:2564eac22e0a 305 if(leg_left_lower.GetLimitUp() == 0)
soulx 12:2564eac22e0a 306 pc.printf("shot\n");
soulx 12:2564eac22e0a 307 else
soulx 12:2564eac22e0a 308 pc.printf("open\n");
soulx 12:2564eac22e0a 309
soulx 12:2564eac22e0a 310 pc.printf("leftLow_limit_down = ");
soulx 12:2564eac22e0a 311 if(leg_left_lower.GetLimitDown() == 0)
soulx 12:2564eac22e0a 312 pc.printf("shot\n");
soulx 12:2564eac22e0a 313 else
soulx 12:2564eac22e0a 314 pc.printf("open\n");
soulx 12:2564eac22e0a 315
soulx 12:2564eac22e0a 316 ///////////////////////////////////////////////////////////////
soulx 12:2564eac22e0a 317 pc.printf("rightUP_limit_up = ");
soulx 12:2564eac22e0a 318 if(leg_right_upper.GetLimitUp() == 0)
soulx 12:2564eac22e0a 319 pc.printf("shot\n");
soulx 12:2564eac22e0a 320 else
soulx 12:2564eac22e0a 321 pc.printf("open\n");
soulx 12:2564eac22e0a 322
soulx 12:2564eac22e0a 323 pc.printf("rightUP_limit_down = ");
soulx 12:2564eac22e0a 324 if(leg_right_upper.GetLimitDown() == 0)
soulx 12:2564eac22e0a 325 pc.printf("shot\n");
soulx 12:2564eac22e0a 326 else
soulx 12:2564eac22e0a 327 pc.printf("open\n");
soulx 12:2564eac22e0a 328
soulx 12:2564eac22e0a 329 pc.printf("rightLOW_limit_up = ");
soulx 12:2564eac22e0a 330 if(leg_right_lower.GetLimitUp() == 0)
soulx 12:2564eac22e0a 331 pc.printf("shot\n");
soulx 12:2564eac22e0a 332 else
soulx 12:2564eac22e0a 333 pc.printf("open\n");
soulx 12:2564eac22e0a 334
soulx 12:2564eac22e0a 335 pc.printf("rightLow_limit_down = ");
soulx 12:2564eac22e0a 336 if(leg_right_lower.GetLimitDown() == 0)
soulx 12:2564eac22e0a 337 pc.printf("shot\n");
soulx 12:2564eac22e0a 338 else
soulx 12:2564eac22e0a 339 pc.printf("open\n");
soulx 12:2564eac22e0a 340
soulx 12:2564eac22e0a 341 pc.printf("\n\n");
soulx 12:2564eac22e0a 342 }
soulx 12:2564eac22e0a 343 }
soulx 12:2564eac22e0a 344
soulx 12:2564eac22e0a 345 void test_motor()
soulx 12:2564eac22e0a 346 {
soulx 12:2564eac22e0a 347 uint8_t state=0;
soulx 12:2564eac22e0a 348
soulx 12:2564eac22e0a 349 pc.printf("TEST MOTOR DELAY\n");
soulx 12:2564eac22e0a 350 t.start();
soulx 12:2564eac22e0a 351 while(1) {
soulx 12:2564eac22e0a 352 if(t.read() > 1.5f) {
soulx 12:2564eac22e0a 353 t.reset();
soulx 12:2564eac22e0a 354 if(state >1) {
soulx 12:2564eac22e0a 355 state =0;
soulx 12:2564eac22e0a 356 } else {
soulx 12:2564eac22e0a 357 state++;
soulx 12:2564eac22e0a 358 }
soulx 12:2564eac22e0a 359 }
soulx 12:2564eac22e0a 360
soulx 12:2564eac22e0a 361 if(state ==0) {
soulx 12:2564eac22e0a 362 leg_left_upper.limit_motor(1);
soulx 12:2564eac22e0a 363 leg_right_upper.limit_motor(1);
soulx 12:2564eac22e0a 364 leg_left_lower.limit_motor(1);
soulx 12:2564eac22e0a 365 leg_right_lower.limit_motor(1);
soulx 12:2564eac22e0a 366 } else {
soulx 12:2564eac22e0a 367 {
soulx 12:2564eac22e0a 368 leg_left_upper.limit_motor(2);
soulx 12:2564eac22e0a 369 leg_right_upper.limit_motor(2);
soulx 12:2564eac22e0a 370 leg_left_lower.limit_motor(2);
soulx 12:2564eac22e0a 371 leg_right_lower.limit_motor(2);
soulx 12:2564eac22e0a 372 }
soulx 12:2564eac22e0a 373 }
soulx 12:2564eac22e0a 374 }
soulx 6:8d80c84e0c09 375 }