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@15:40ccb6433500, 2015-12-07 (annotated)

Committer:
soulx
Date:
Mon Dec 07 09:16:24 2015 +0000
Revision:
15:40ccb6433500
Parent:
14:7fe99764b2d0 
????????????????

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 14:7fe99764b2d0 11 #include "QEI.h"
soulx 14:7fe99764b2d0 12
soulx 6:8d80c84e0c09 13 //set frequancy unit in Hz
soulx 6:8d80c84e0c09 14 #define F_UPDATE 10.0f
soulx 6:8d80c84e0c09 15 #define TIMER_UPDATE 1.0f/F_UPDATE
soulx 6:8d80c84e0c09 16
soulx 6:8d80c84e0c09 17 //counter not receive from station
soulx 6:8d80c84e0c09 18 #define TIMEOUT_RESPONE_COMMAND 5
soulx 10:53cb691e22bf 19
soulx 14:7fe99764b2d0 20 //
soulx 14:7fe99764b2d0 21 #define PULSE_RESOLUTION 500
soulx 14:7fe99764b2d0 22
soulx 10:53cb691e22bf 23 MOTION_CONTROL leg_left_upper(INA_L_U,INB_L_U,LIMIT_LU_U,LIMIT_LU_D,VR_LU);
soulx 10:53cb691e22bf 24 MOTION_CONTROL leg_left_lower(INA_L_L,INB_L_L,LIMIT_LL_U,LIMIT_LL_D,VR_LL);
soulx 10:53cb691e22bf 25 MOTION_CONTROL leg_right_upper(INA_R_U,INB_R_U,LIMIT_RU_U,LIMIT_RU_D,VR_RU);
soulx 10:53cb691e22bf 26 MOTION_CONTROL leg_right_lower(INA_R_L,INB_R_L,LIMIT_RL_U,LIMIT_RL_D,VR_RL);
soulx 10:53cb691e22bf 27
soulx 0:2433ddae2772 28 DigitalOut myled(LED1);
soulx 0:2433ddae2772 29 DigitalIn mybutton(USER_BUTTON);
soulx 0:2433ddae2772 30
soulx 6:8d80c84e0c09 31 //communication config
soulx 6:8d80c84e0c09 32 //serial for debug
soulx 13:218c22a620cc 33 Serial pc(USBTX, USBRX);
soulx 6:8d80c84e0c09 34 //serial for xbee
soulx 6:8d80c84e0c09 35 COMMUNICATION pan_a(TX_XBEE, RX_XBEE,115200,1000,1000); // tx, rx
soulx 0:2433ddae2772 36
soulx 14:7fe99764b2d0 37 QEI wheel (ENCODE_A, ENCODE_B, ENCODE_Z, PULSE_RESOLUTION);
soulx 14:7fe99764b2d0 38
soulx 11:336dd293daa1 39 //Fuction prototye
soulx 11:336dd293daa1 40 void getCommand();
soulx 11:336dd293daa1 41 // init function
soulx 11:336dd293daa1 42 void calibration();
soulx 11:336dd293daa1 43 void test_position();
soulx 12:2564eac22e0a 44 void test_status();
soulx 14:7fe99764b2d0 45 void test_sleep();
soulx 14:7fe99764b2d0 46 void test_sit();
soulx 14:7fe99764b2d0 47 void test_stand();
soulx 14:7fe99764b2d0 48
soulx 12:2564eac22e0a 49 void test_limit();
soulx 12:2564eac22e0a 50 void test_motor();
soulx 0:2433ddae2772 51
soulx 12:2564eac22e0a 52 void routine();
soulx 12:2564eac22e0a 53
soulx 13:218c22a620cc 54 void serial_control();
soulx 13:218c22a620cc 55
soulx 12:2564eac22e0a 56 void management(ANDANTE_PROTOCOL_PACKET *packet);
soulx 0:2433ddae2772 57
soulx 11:336dd293daa1 58 void copy_data(ANDANTE_PROTOCOL_PACKET *scr, ANDANTE_PROTOCOL_PACKET *dst);
soulx 0:2433ddae2772 59
soulx 15:40ccb6433500 60 void version();
soulx 6:8d80c84e0c09 61 //set foreground
soulx 6:8d80c84e0c09 62 Ticker Update_command;
soulx 6:8d80c84e0c09 63
soulx 11:336dd293daa1 64 Timer t;
soulx 11:336dd293daa1 65
soulx 12:2564eac22e0a 66 //struct
soulx 12:2564eac22e0a 67 struct PARAM_WRITE {
soulx 12:2564eac22e0a 68 uint16_t left_up;
soulx 12:2564eac22e0a 69 uint16_t left_down;
soulx 12:2564eac22e0a 70 uint16_t right_up;
soulx 12:2564eac22e0a 71 uint16_t right_down;
soulx 12:2564eac22e0a 72 };
soulx 12:2564eac22e0a 73
soulx 11:336dd293daa1 74 //variable
soulx 12:2564eac22e0a 75 //volatile ANDANTE_PROTOCOL_PACKET *param;
soulx 11:336dd293daa1 76 volatile uint8_t status=0;
soulx 12:2564eac22e0a 77 //volatile PARAM_WRITE buff;
soulx 15:40ccb6433500 78 PARAM_WRITE buff,buff2;
soulx 6:8d80c84e0c09 79
soulx 13:218c22a620cc 80 int serial_data;
soulx 13:218c22a620cc 81
soulx 0:2433ddae2772 82 //Main function
soulx 0:2433ddae2772 83 int main()
soulx 0:2433ddae2772 84 {
soulx 12:2564eac22e0a 85 //int state=0;
soulx 12:2564eac22e0a 86
soulx 11:336dd293daa1 87 pc.baud(115200);
soulx 11:336dd293daa1 88 pc.printf("Welcome to DOGWHEELSCHAIR\n");
soulx 12:2564eac22e0a 89
soulx 14:7fe99764b2d0 90 buff.left_up = 63;
soulx 14:7fe99764b2d0 91 buff.right_up =63;
soulx 14:7fe99764b2d0 92 buff.left_down = 63;
soulx 14:7fe99764b2d0 93 buff.right_down = 63;
soulx 12:2564eac22e0a 94
soulx 11:336dd293daa1 95 if(mybutton == 0) {
soulx 11:336dd293daa1 96 calibration();
soulx 11:336dd293daa1 97 } else {
soulx 11:336dd293daa1 98 pc.printf("Lock position Min-Max...");
soulx 15:40ccb6433500 99 leg_left_upper.SetMaxPosition(62463);
soulx 15:40ccb6433500 100 leg_left_upper.SetMinPosition(29319);
soulx 11:336dd293daa1 101
soulx 15:40ccb6433500 102 leg_left_lower.SetMaxPosition(43450);
soulx 15:40ccb6433500 103 leg_left_lower.SetMinPosition(288);
soulx 11:336dd293daa1 104
soulx 15:40ccb6433500 105 leg_right_upper.SetMaxPosition(42794);
soulx 15:40ccb6433500 106 leg_right_upper.SetMinPosition(11282);
soulx 11:336dd293daa1 107
soulx 15:40ccb6433500 108 leg_right_lower.SetMaxPosition(37001);
soulx 15:40ccb6433500 109 leg_right_lower.SetMinPosition(144);
soulx 11:336dd293daa1 110 pc.printf("pass\n");
soulx 11:336dd293daa1 111 }
soulx 11:336dd293daa1 112
soulx 11:336dd293daa1 113 Update_command.attach(&getCommand,TIMER_UPDATE);
soulx 11:336dd293daa1 114
soulx 15:40ccb6433500 115 version();
soulx 13:218c22a620cc 116 serial_control();
soulx 13:218c22a620cc 117 //test_motor();
soulx 13:218c22a620cc 118 //test_limit();
soulx 12:2564eac22e0a 119 test_status();
soulx 12:2564eac22e0a 120 routine();
soulx 11:336dd293daa1 121 }
soulx 11:336dd293daa1 122
soulx 11:336dd293daa1 123 void getCommand()
soulx 11:336dd293daa1 124 {
soulx 11:336dd293daa1 125 static uint8_t count =0;
soulx 11:336dd293daa1 126
soulx 11:336dd293daa1 127 ANDANTE_PROTOCOL_PACKET packet;
soulx 11:336dd293daa1 128
soulx 11:336dd293daa1 129 uint8_t status = pan_a.receiveCommunicatePacket(&packet);
soulx 11:336dd293daa1 130 myled=0;
soulx 11:336dd293daa1 131
soulx 11:336dd293daa1 132 if(status == ANDANTE_ERRBIT_NONE) {
soulx 11:336dd293daa1 133 if(count >2 && count <10) {
soulx 11:336dd293daa1 134 count--;
soulx 11:336dd293daa1 135 } else {
soulx 11:336dd293daa1 136 count=0;
soulx 5:f07cbb5a86c3 137 }
soulx 11:336dd293daa1 138
soulx 11:336dd293daa1 139
soulx 12:2564eac22e0a 140 //pan_a.sendCommunicatePacket(&packet);
soulx 11:336dd293daa1 141
soulx 11:336dd293daa1 142 //update command
soulx 11:336dd293daa1 143 //setControl(&packet);
soulx 11:336dd293daa1 144
soulx 12:2564eac22e0a 145
soulx 11:336dd293daa1 146 } else if(status == ANDANTE_ERRBIT_READ_TIMEOUT) {
soulx 11:336dd293daa1 147 count++;
soulx 11:336dd293daa1 148 }
soulx 5:f07cbb5a86c3 149
soulx 11:336dd293daa1 150 if(count >= TIMEOUT_RESPONE_COMMAND) {
soulx 11:336dd293daa1 151 //stop robot
soulx 11:336dd293daa1 152 count++;
soulx 11:336dd293daa1 153 myled=1;
soulx 11:336dd293daa1 154 // setSpeedControl(0.0);
soulx 11:336dd293daa1 155 }
soulx 11:336dd293daa1 156 }
soulx 0:2433ddae2772 157
soulx 11:336dd293daa1 158 void calibration()
soulx 11:336dd293daa1 159 {
soulx 11:336dd293daa1 160 //calibration
soulx 11:336dd293daa1 161 pc.printf("Calibration [START]\n");
soulx 11:336dd293daa1 162 leg_left_upper.calibration();
soulx 11:336dd293daa1 163 pc.printf("Left_UPPER\n");
soulx 11:336dd293daa1 164 pc.printf("max_position = %d\n",leg_left_upper.GetMaxPosition());
soulx 11:336dd293daa1 165 pc.printf("min_position = %d\n",leg_left_upper.GetMinPosition());
soulx 11:336dd293daa1 166 leg_left_lower.calibration();
soulx 11:336dd293daa1 167 pc.printf("Left_Lower\n");
soulx 11:336dd293daa1 168 pc.printf("max_position = %d\n",leg_left_lower.GetMaxPosition());
soulx 11:336dd293daa1 169 pc.printf("min_position = %d\n",leg_left_lower.GetMinPosition());
soulx 11:336dd293daa1 170 leg_right_upper.calibration();
soulx 11:336dd293daa1 171 pc.printf("right_UPPER\n");
soulx 11:336dd293daa1 172 pc.printf("max_position = %d\n",leg_right_upper.GetMaxPosition());
soulx 11:336dd293daa1 173 pc.printf("min_position = %d\n",leg_right_upper.GetMinPosition());
soulx 11:336dd293daa1 174 leg_right_lower.calibration();
soulx 11:336dd293daa1 175 pc.printf("right_Lower\n");
soulx 11:336dd293daa1 176 pc.printf("max_position = %d\n",leg_right_lower.GetMaxPosition());
soulx 11:336dd293daa1 177 pc.printf("min_position = %d\n",leg_right_lower.GetMinPosition());
soulx 0:2433ddae2772 178
soulx 11:336dd293daa1 179 pc.printf("Calibration [FINISH]\n");
soulx 11:336dd293daa1 180 pc.printf("RUN mode [START]\n");
soulx 11:336dd293daa1 181 wait(1);
soulx 11:336dd293daa1 182 }
soulx 11:336dd293daa1 183
soulx 11:336dd293daa1 184 void test_position()
soulx 11:336dd293daa1 185 {
soulx 11:336dd293daa1 186 uint8_t state;
soulx 11:336dd293daa1 187 do {
soulx 10:53cb691e22bf 188 /*
soulx 10:53cb691e22bf 189 state=0;
soulx 10:53cb691e22bf 190 //leg_left_upper.position_control(500);
soulx 10:53cb691e22bf 191 if(leg_left_lower.position_control(500) ==2)
soulx 10:53cb691e22bf 192 state++;
soulx 10:53cb691e22bf 193 if(leg_right_upper.position_control(500) == 2)
soulx 10:53cb691e22bf 194 state++;
soulx 10:53cb691e22bf 195 if(leg_right_lower.position_control(500) == 2)
soulx 10:53cb691e22bf 196 state++;
soulx 10:53cb691e22bf 197 */
soulx 10:53cb691e22bf 198 state = leg_left_upper.position_control(32);
soulx 10:53cb691e22bf 199 pc.printf("state_lu %d\n",state);
soulx 10:53cb691e22bf 200 state = leg_left_lower.position_control(32);
soulx 10:53cb691e22bf 201 pc.printf("state_ll %d\n",state);
soulx 10:53cb691e22bf 202 state = leg_right_upper.position_control(32);
soulx 10:53cb691e22bf 203 pc.printf("state_ru %d\n",state);
soulx 11:336dd293daa1 204 state = leg_right_lower.position_control(32);
soulx 10:53cb691e22bf 205 pc.printf("state_rl %d\n",state);
soulx 10:53cb691e22bf 206 state=0;
soulx 11:336dd293daa1 207 } while(state != 2);
soulx 10:53cb691e22bf 208 pc.printf("[Finish test]\n");
soulx 12:2564eac22e0a 209 }
soulx 12:2564eac22e0a 210
soulx 12:2564eac22e0a 211 void test_status()
soulx 14:7fe99764b2d0 212 {
soulx 12:2564eac22e0a 213 uint16_t state=0;
soulx 13:218c22a620cc 214 pc.printf("Test_status\n");
soulx 12:2564eac22e0a 215 t.start();
soulx 12:2564eac22e0a 216 while(1) {
soulx 12:2564eac22e0a 217
soulx 13:218c22a620cc 218 if(pc.readable() == 1) {
soulx 13:218c22a620cc 219 serial_data = pc.getc();
soulx 13:218c22a620cc 220 }
soulx 12:2564eac22e0a 221
soulx 12:2564eac22e0a 222 if(t.read() > 10.0f) {
soulx 12:2564eac22e0a 223 t.reset();
soulx 12:2564eac22e0a 224 if(status >3) {
soulx 12:2564eac22e0a 225 status =0;
soulx 12:2564eac22e0a 226 } else {
soulx 12:2564eac22e0a 227 status++;
soulx 12:2564eac22e0a 228 }
soulx 12:2564eac22e0a 229 }
soulx 12:2564eac22e0a 230
soulx 12:2564eac22e0a 231 if(status == 0) {
soulx 12:2564eac22e0a 232 state =0;
soulx 12:2564eac22e0a 233 // sleep
soulx 12:2564eac22e0a 234 state += leg_left_upper.position_control(0);
soulx 12:2564eac22e0a 235 state += leg_right_upper.position_control(0);
soulx 12:2564eac22e0a 236 state += leg_left_lower.position_control(64);
soulx 12:2564eac22e0a 237 state += leg_right_lower.position_control(64);
soulx 12:2564eac22e0a 238 if(state == 8) {
soulx 12:2564eac22e0a 239 myled=1;
soulx 12:2564eac22e0a 240 } else {
soulx 12:2564eac22e0a 241 myled=0;
soulx 12:2564eac22e0a 242 }
soulx 12:2564eac22e0a 243 } else if(status == 1 || status ==3) {
soulx 12:2564eac22e0a 244 state =0;
soulx 12:2564eac22e0a 245 // sit
soulx 12:2564eac22e0a 246 state += leg_left_upper.position_control(64);
soulx 12:2564eac22e0a 247 state += leg_right_upper.position_control(64);
soulx 12:2564eac22e0a 248 state += leg_left_lower.position_control(64);
soulx 12:2564eac22e0a 249 state += leg_right_lower.position_control(64);
soulx 12:2564eac22e0a 250 if(state == 8) {
soulx 12:2564eac22e0a 251 myled=1;
soulx 12:2564eac22e0a 252 } else {
soulx 12:2564eac22e0a 253 myled=0;
soulx 12:2564eac22e0a 254 }
soulx 12:2564eac22e0a 255 } else if(status == 2) {
soulx 12:2564eac22e0a 256 state =0;
soulx 12:2564eac22e0a 257 // stand
soulx 12:2564eac22e0a 258 state += leg_left_upper.position_control(64);
soulx 12:2564eac22e0a 259 state += leg_right_upper.position_control(64);
soulx 12:2564eac22e0a 260 state += leg_left_lower.position_control(0);
soulx 12:2564eac22e0a 261 state += leg_right_lower.position_control(0);
soulx 12:2564eac22e0a 262 if(state == 8) {
soulx 12:2564eac22e0a 263 myled=1;
soulx 12:2564eac22e0a 264 } else {
soulx 12:2564eac22e0a 265 myled=0;
soulx 12:2564eac22e0a 266 }
soulx 12:2564eac22e0a 267 }
soulx 12:2564eac22e0a 268 }
soulx 12:2564eac22e0a 269 //t.stop();
soulx 12:2564eac22e0a 270 }
soulx 12:2564eac22e0a 271
soulx 12:2564eac22e0a 272 void management(ANDANTE_PROTOCOL_PACKET *packet)
soulx 12:2564eac22e0a 273 {
soulx 12:2564eac22e0a 274 switch(packet->instructionErrorId) {
soulx 12:2564eac22e0a 275 case 0x01:
soulx 12:2564eac22e0a 276 buff.left_up = Utilities::ConvertUInt8ArrayToInt16(packet->parameter);
soulx 12:2564eac22e0a 277 break;
soulx 12:2564eac22e0a 278 case 0x02:
soulx 12:2564eac22e0a 279 buff.left_down = Utilities::ConvertUInt8ArrayToInt16(packet->parameter);
soulx 12:2564eac22e0a 280 break;
soulx 12:2564eac22e0a 281 case 0x03:
soulx 12:2564eac22e0a 282 buff.right_up = Utilities::ConvertUInt8ArrayToInt16(packet->parameter);
soulx 12:2564eac22e0a 283 break;
soulx 12:2564eac22e0a 284 case 0x04:
soulx 12:2564eac22e0a 285 buff.right_down = Utilities::ConvertUInt8ArrayToInt16(packet->parameter);
soulx 12:2564eac22e0a 286 break;
soulx 12:2564eac22e0a 287 }
soulx 12:2564eac22e0a 288 }
soulx 12:2564eac22e0a 289
soulx 12:2564eac22e0a 290 void routine()
soulx 12:2564eac22e0a 291 {
soulx 12:2564eac22e0a 292 uint8_t state=0;
soulx 12:2564eac22e0a 293 // PARAM_WRITE pos = buff;
soulx 12:2564eac22e0a 294 while(1) {
soulx 12:2564eac22e0a 295 state =0;
soulx 12:2564eac22e0a 296 // sit
soulx 12:2564eac22e0a 297 state += leg_left_upper.position_control(buff.left_up);
soulx 12:2564eac22e0a 298 state += leg_right_upper.position_control(buff.right_up);
soulx 12:2564eac22e0a 299 state += leg_left_lower.position_control(buff.left_down);
soulx 12:2564eac22e0a 300 state += leg_right_lower.position_control(buff.right_down);
soulx 12:2564eac22e0a 301 if(state == 8) {
soulx 12:2564eac22e0a 302 myled=1;
soulx 12:2564eac22e0a 303 } else {
soulx 12:2564eac22e0a 304 myled=0;
soulx 12:2564eac22e0a 305 }
soulx 12:2564eac22e0a 306 }
soulx 12:2564eac22e0a 307 }
soulx 12:2564eac22e0a 308
soulx 12:2564eac22e0a 309 void test_limit()
soulx 12:2564eac22e0a 310 {
soulx 12:2564eac22e0a 311 pc.printf("TEST LIMIT ALL\n");
soulx 15:40ccb6433500 312
soulx 14:7fe99764b2d0 313 // while(1) {
soulx 15:40ccb6433500 314 if(t.read() > 1.0f) {
soulx 15:40ccb6433500 315 t.reset();
soulx 15:40ccb6433500 316 pc.printf("leftUP_limit_up = ");
soulx 15:40ccb6433500 317 if(leg_left_upper.GetLimitUp() == 1)
soulx 15:40ccb6433500 318 pc.printf("shot\n");
soulx 15:40ccb6433500 319 else
soulx 15:40ccb6433500 320 pc.printf("open\n");
soulx 12:2564eac22e0a 321
soulx 15:40ccb6433500 322 pc.printf("leftUP_limit_down = ");
soulx 15:40ccb6433500 323 if(leg_left_upper.GetLimitDown() == 1)
soulx 15:40ccb6433500 324 pc.printf("shot\n");
soulx 15:40ccb6433500 325 else
soulx 15:40ccb6433500 326 pc.printf("open\n");
soulx 12:2564eac22e0a 327
soulx 15:40ccb6433500 328 pc.printf("leftLOW_limit_up = ");
soulx 15:40ccb6433500 329 if(leg_left_lower.GetLimitUp() == 1)
soulx 15:40ccb6433500 330 pc.printf("shot\n");
soulx 15:40ccb6433500 331 else
soulx 15:40ccb6433500 332 pc.printf("open\n");
soulx 12:2564eac22e0a 333
soulx 15:40ccb6433500 334 pc.printf("leftLow_limit_down = ");
soulx 15:40ccb6433500 335 if(leg_left_lower.GetLimitDown() == 1)
soulx 15:40ccb6433500 336 pc.printf("shot\n");
soulx 15:40ccb6433500 337 else
soulx 15:40ccb6433500 338 pc.printf("open\n");
soulx 12:2564eac22e0a 339
soulx 12:2564eac22e0a 340 ///////////////////////////////////////////////////////////////
soulx 15:40ccb6433500 341 pc.printf("rightUP_limit_up = ");
soulx 15:40ccb6433500 342 if(leg_right_upper.GetLimitUp() == 1)
soulx 15:40ccb6433500 343 pc.printf("shot\n");
soulx 15:40ccb6433500 344 else
soulx 15:40ccb6433500 345 pc.printf("open\n");
soulx 12:2564eac22e0a 346
soulx 15:40ccb6433500 347 pc.printf("rightUP_limit_down = ");
soulx 15:40ccb6433500 348 if(leg_right_upper.GetLimitDown() == 1)
soulx 15:40ccb6433500 349 pc.printf("shot\n");
soulx 15:40ccb6433500 350 else
soulx 15:40ccb6433500 351 pc.printf("open\n");
soulx 12:2564eac22e0a 352
soulx 15:40ccb6433500 353 pc.printf("rightLOW_limit_up = ");
soulx 15:40ccb6433500 354 if(leg_right_lower.GetLimitUp() == 1)
soulx 15:40ccb6433500 355 pc.printf("shot\n");
soulx 15:40ccb6433500 356 else
soulx 15:40ccb6433500 357 pc.printf("open\n");
soulx 12:2564eac22e0a 358
soulx 15:40ccb6433500 359 pc.printf("rightLow_limit_down = ");
soulx 15:40ccb6433500 360 if(leg_right_lower.GetLimitDown() == 1)
soulx 15:40ccb6433500 361 pc.printf("shot\n");
soulx 15:40ccb6433500 362 else
soulx 15:40ccb6433500 363 pc.printf("open\n");
soulx 12:2564eac22e0a 364
soulx 15:40ccb6433500 365 pc.printf("\n\n");
soulx 15:40ccb6433500 366 }
soulx 15:40ccb6433500 367 // }
soulx 15:40ccb6433500 368 t.stop();
soulx 12:2564eac22e0a 369 }
soulx 12:2564eac22e0a 370
soulx 12:2564eac22e0a 371 void test_motor()
soulx 12:2564eac22e0a 372 {
soulx 12:2564eac22e0a 373 uint8_t state=0;
soulx 12:2564eac22e0a 374
soulx 12:2564eac22e0a 375 pc.printf("TEST MOTOR DELAY\n");
soulx 12:2564eac22e0a 376 t.start();
soulx 12:2564eac22e0a 377 while(1) {
soulx 12:2564eac22e0a 378 if(t.read() > 1.5f) {
soulx 12:2564eac22e0a 379 t.reset();
soulx 12:2564eac22e0a 380 if(state >1) {
soulx 12:2564eac22e0a 381 state =0;
soulx 12:2564eac22e0a 382 } else {
soulx 12:2564eac22e0a 383 state++;
soulx 12:2564eac22e0a 384 }
soulx 12:2564eac22e0a 385 }
soulx 12:2564eac22e0a 386
soulx 12:2564eac22e0a 387 if(state ==0) {
soulx 12:2564eac22e0a 388 leg_left_upper.limit_motor(1);
soulx 12:2564eac22e0a 389 leg_right_upper.limit_motor(1);
soulx 12:2564eac22e0a 390 leg_left_lower.limit_motor(1);
soulx 12:2564eac22e0a 391 leg_right_lower.limit_motor(1);
soulx 12:2564eac22e0a 392 } else {
soulx 12:2564eac22e0a 393 {
soulx 12:2564eac22e0a 394 leg_left_upper.limit_motor(2);
soulx 12:2564eac22e0a 395 leg_right_upper.limit_motor(2);
soulx 12:2564eac22e0a 396 leg_left_lower.limit_motor(2);
soulx 12:2564eac22e0a 397 leg_right_lower.limit_motor(2);
soulx 12:2564eac22e0a 398 }
soulx 12:2564eac22e0a 399 }
soulx 12:2564eac22e0a 400 }
soulx 13:218c22a620cc 401 }
soulx 13:218c22a620cc 402
soulx 13:218c22a620cc 403 void serial_control()
soulx 13:218c22a620cc 404 {
soulx 13:218c22a620cc 405
soulx 14:7fe99764b2d0 406 uint16_t left_up=63;
soulx 13:218c22a620cc 407 uint16_t left_down=0;
soulx 14:7fe99764b2d0 408 uint16_t right_up=63;
soulx 13:218c22a620cc 409 uint16_t right_down=0;
soulx 13:218c22a620cc 410
soulx 14:7fe99764b2d0 411 uint16_t offset_ll=11400;
soulx 14:7fe99764b2d0 412 uint16_t offset_rl=17800;
soulx 13:218c22a620cc 413
soulx 13:218c22a620cc 414 uint8_t state=0;
soulx 13:218c22a620cc 415
soulx 13:218c22a620cc 416 t.start();
soulx 13:218c22a620cc 417
soulx 13:218c22a620cc 418 while(1) {
soulx 13:218c22a620cc 419 if(pc.readable() == 1) {
soulx 13:218c22a620cc 420 serial_data = pc.getc();
soulx 13:218c22a620cc 421 }
soulx 13:218c22a620cc 422
soulx 13:218c22a620cc 423 switch(serial_data) {
soulx 13:218c22a620cc 424 case 'q':
soulx 14:7fe99764b2d0 425 if(left_up >=63) {
soulx 13:218c22a620cc 426 left_up= 63;
soulx 13:218c22a620cc 427 } else {
soulx 13:218c22a620cc 428 left_up++;
soulx 13:218c22a620cc 429 }
soulx 14:7fe99764b2d0 430 serial_data=0;
soulx 13:218c22a620cc 431 break;
soulx 13:218c22a620cc 432
soulx 13:218c22a620cc 433 case 'a':
soulx 14:7fe99764b2d0 434 if(left_up >=63 || left_up <=0) {
soulx 13:218c22a620cc 435 left_up= 0;
soulx 13:218c22a620cc 436 } else {
soulx 13:218c22a620cc 437 left_up--;
soulx 13:218c22a620cc 438 }
soulx 14:7fe99764b2d0 439 serial_data=0;
soulx 13:218c22a620cc 440 break;
soulx 13:218c22a620cc 441
soulx 13:218c22a620cc 442 case 'z':
soulx 14:7fe99764b2d0 443 if(left_up >=53) {
soulx 13:218c22a620cc 444 left_up= 63;
soulx 13:218c22a620cc 445 } else {
soulx 13:218c22a620cc 446 left_up+=10;
soulx 13:218c22a620cc 447 }
soulx 14:7fe99764b2d0 448 serial_data=0;
soulx 13:218c22a620cc 449 break;
soulx 13:218c22a620cc 450
soulx 13:218c22a620cc 451 case 'Z':
soulx 14:7fe99764b2d0 452 if(left_up >=53 || left_up <=10) {
soulx 13:218c22a620cc 453 left_up= 0;
soulx 13:218c22a620cc 454 } else {
soulx 13:218c22a620cc 455 left_up-=10;
soulx 13:218c22a620cc 456 }
soulx 14:7fe99764b2d0 457 serial_data=0;
soulx 13:218c22a620cc 458 break;
soulx 13:218c22a620cc 459
soulx 13:218c22a620cc 460 case 'w':
soulx 14:7fe99764b2d0 461 if(left_down >=63) {
soulx 13:218c22a620cc 462 left_down= 63;
soulx 13:218c22a620cc 463 } else {
soulx 13:218c22a620cc 464 left_down++;
soulx 13:218c22a620cc 465 }
soulx 14:7fe99764b2d0 466 serial_data=0;
soulx 13:218c22a620cc 467 break;
soulx 13:218c22a620cc 468
soulx 13:218c22a620cc 469 case 's':
soulx 14:7fe99764b2d0 470 if(left_down >=63 || left_down <=0) {
soulx 13:218c22a620cc 471 left_down= 0;
soulx 13:218c22a620cc 472 } else {
soulx 13:218c22a620cc 473 left_down--;
soulx 13:218c22a620cc 474 }
soulx 14:7fe99764b2d0 475 serial_data=0;
soulx 13:218c22a620cc 476 break;
soulx 13:218c22a620cc 477
soulx 13:218c22a620cc 478 case 'x':
soulx 14:7fe99764b2d0 479 if(left_down >=53) {
soulx 13:218c22a620cc 480 left_down= 63;
soulx 13:218c22a620cc 481 } else {
soulx 13:218c22a620cc 482 left_down+=10;
soulx 13:218c22a620cc 483 }
soulx 14:7fe99764b2d0 484 serial_data=0;
soulx 13:218c22a620cc 485 break;
soulx 13:218c22a620cc 486
soulx 13:218c22a620cc 487 case 'X':
soulx 14:7fe99764b2d0 488 if(left_down >=53 || left_down <=10) {
soulx 13:218c22a620cc 489 left_down= 0;
soulx 13:218c22a620cc 490 } else {
soulx 13:218c22a620cc 491 left_down-=10;
soulx 13:218c22a620cc 492 }
soulx 14:7fe99764b2d0 493 serial_data=0;
soulx 13:218c22a620cc 494 break;
soulx 13:218c22a620cc 495
soulx 13:218c22a620cc 496
soulx 13:218c22a620cc 497 case 'e':
soulx 14:7fe99764b2d0 498 if(right_up >=63) {
soulx 13:218c22a620cc 499 right_up= 63;
soulx 13:218c22a620cc 500 } else {
soulx 13:218c22a620cc 501 right_up++;
soulx 13:218c22a620cc 502 }
soulx 14:7fe99764b2d0 503 serial_data=0;
soulx 13:218c22a620cc 504 break;
soulx 13:218c22a620cc 505
soulx 13:218c22a620cc 506 case 'd':
soulx 14:7fe99764b2d0 507 if(right_up >=63 || right_up <=0) {
soulx 13:218c22a620cc 508 right_up= 0;
soulx 13:218c22a620cc 509 } else {
soulx 13:218c22a620cc 510 right_up--;
soulx 13:218c22a620cc 511 }
soulx 14:7fe99764b2d0 512 serial_data=0;
soulx 13:218c22a620cc 513 break;
soulx 13:218c22a620cc 514
soulx 13:218c22a620cc 515 case 'c':
soulx 14:7fe99764b2d0 516 if(right_up >=53) {
soulx 13:218c22a620cc 517 right_up= 63;
soulx 13:218c22a620cc 518 } else {
soulx 13:218c22a620cc 519 right_up+=10;
soulx 13:218c22a620cc 520 }
soulx 14:7fe99764b2d0 521 serial_data=0;
soulx 13:218c22a620cc 522 break;
soulx 13:218c22a620cc 523
soulx 13:218c22a620cc 524 case 'C':
soulx 14:7fe99764b2d0 525 if(right_up >=53 || right_up <=10) {
soulx 13:218c22a620cc 526 right_up= 0;
soulx 13:218c22a620cc 527 } else {
soulx 13:218c22a620cc 528 right_up-=10;
soulx 13:218c22a620cc 529 }
soulx 14:7fe99764b2d0 530 serial_data=0;
soulx 13:218c22a620cc 531 break;
soulx 13:218c22a620cc 532
soulx 13:218c22a620cc 533
soulx 13:218c22a620cc 534
soulx 13:218c22a620cc 535
soulx 13:218c22a620cc 536 case 'r':
soulx 14:7fe99764b2d0 537 if(right_down >=63) {
soulx 13:218c22a620cc 538 right_down= 63;
soulx 13:218c22a620cc 539 } else {
soulx 13:218c22a620cc 540 right_down++;
soulx 13:218c22a620cc 541 }
soulx 14:7fe99764b2d0 542 serial_data=0;
soulx 13:218c22a620cc 543 break;
soulx 13:218c22a620cc 544
soulx 13:218c22a620cc 545 case 'f':
soulx 14:7fe99764b2d0 546 if(right_down >=63 || right_down <=0) {
soulx 13:218c22a620cc 547 right_down= 0;
soulx 13:218c22a620cc 548 } else {
soulx 13:218c22a620cc 549 right_down--;
soulx 13:218c22a620cc 550 }
soulx 14:7fe99764b2d0 551 serial_data=0;
soulx 14:7fe99764b2d0 552
soulx 13:218c22a620cc 553 break;
soulx 13:218c22a620cc 554
soulx 13:218c22a620cc 555 case 'v':
soulx 14:7fe99764b2d0 556 if(right_down >=53) {
soulx 13:218c22a620cc 557 right_down= 63;
soulx 13:218c22a620cc 558 } else {
soulx 13:218c22a620cc 559 right_down+=10;
soulx 13:218c22a620cc 560 }
soulx 14:7fe99764b2d0 561 serial_data=0;
soulx 14:7fe99764b2d0 562
soulx 13:218c22a620cc 563 break;
soulx 13:218c22a620cc 564
soulx 13:218c22a620cc 565 case 'V':
soulx 14:7fe99764b2d0 566 if(right_down >=53 || right_down <=10) {
soulx 13:218c22a620cc 567 right_down= 0;
soulx 13:218c22a620cc 568 } else {
soulx 13:218c22a620cc 569 right_down-=10;
soulx 13:218c22a620cc 570 }
soulx 14:7fe99764b2d0 571 serial_data=0;
soulx 13:218c22a620cc 572 break;
soulx 13:218c22a620cc 573
soulx 13:218c22a620cc 574 case 'p':
soulx 13:218c22a620cc 575 test_status();
soulx 13:218c22a620cc 576 break;
soulx 13:218c22a620cc 577
soulx 14:7fe99764b2d0 578 case '7':
soulx 14:7fe99764b2d0 579 test_sleep();
soulx 14:7fe99764b2d0 580 break;
soulx 14:7fe99764b2d0 581
soulx 14:7fe99764b2d0 582 case '8':
soulx 14:7fe99764b2d0 583 test_sit();
soulx 14:7fe99764b2d0 584 break;
soulx 14:7fe99764b2d0 585
soulx 14:7fe99764b2d0 586 case '9':
soulx 14:7fe99764b2d0 587 test_stand();
soulx 14:7fe99764b2d0 588 break;
soulx 14:7fe99764b2d0 589
soulx 13:218c22a620cc 590 case 'y':
soulx 13:218c22a620cc 591 if(offset_ll > 30000) {
soulx 13:218c22a620cc 592 offset_ll =30000;
soulx 13:218c22a620cc 593 } else {
soulx 13:218c22a620cc 594
soulx 13:218c22a620cc 595 offset_ll+=100;
soulx 13:218c22a620cc 596 }
soulx 13:218c22a620cc 597 leg_left_lower.SetOffset(offset_ll);
soulx 14:7fe99764b2d0 598 serial_data=0;
soulx 13:218c22a620cc 599 break;
soulx 13:218c22a620cc 600
soulx 13:218c22a620cc 601 case 'h':
soulx 13:218c22a620cc 602 if(offset_ll <= 100) {
soulx 13:218c22a620cc 603 offset_ll =0;
soulx 13:218c22a620cc 604 } else {
soulx 13:218c22a620cc 605
soulx 13:218c22a620cc 606 offset_ll-=100;
soulx 13:218c22a620cc 607 }
soulx 13:218c22a620cc 608 leg_left_lower.SetOffset(offset_ll);
soulx 14:7fe99764b2d0 609 serial_data=0;
soulx 13:218c22a620cc 610 break;
soulx 13:218c22a620cc 611
soulx 13:218c22a620cc 612 case 'u':
soulx 13:218c22a620cc 613 if(offset_rl > 30000) {
soulx 13:218c22a620cc 614 offset_rl =30000;
soulx 13:218c22a620cc 615 } else {
soulx 13:218c22a620cc 616
soulx 13:218c22a620cc 617 offset_rl+=100;
soulx 13:218c22a620cc 618 }
soulx 13:218c22a620cc 619 leg_right_lower.SetOffset(offset_rl);
soulx 14:7fe99764b2d0 620 serial_data=0;
soulx 13:218c22a620cc 621 break;
soulx 13:218c22a620cc 622
soulx 13:218c22a620cc 623 case 'j':
soulx 13:218c22a620cc 624 if(offset_rl <= 100) {
soulx 13:218c22a620cc 625 offset_rl =0;
soulx 13:218c22a620cc 626 } else {
soulx 13:218c22a620cc 627
soulx 13:218c22a620cc 628 offset_rl-=100;
soulx 13:218c22a620cc 629 }
soulx 13:218c22a620cc 630 leg_right_lower.SetOffset(offset_rl);
soulx 14:7fe99764b2d0 631 serial_data=0;
soulx 13:218c22a620cc 632 break;
soulx 13:218c22a620cc 633
soulx 14:7fe99764b2d0 634 case '0':
soulx 13:218c22a620cc 635 leg_left_lower.SetMode(0);
soulx 13:218c22a620cc 636 leg_right_lower.SetMode(0);
soulx 13:218c22a620cc 637 pc.printf("MODE 0:ON\n");
soulx 14:7fe99764b2d0 638 serial_data=0;
soulx 13:218c22a620cc 639 break;
soulx 13:218c22a620cc 640 case '1':
soulx 13:218c22a620cc 641 leg_left_lower.SetMode(1);
soulx 13:218c22a620cc 642 leg_right_lower.SetMode(1);
soulx 13:218c22a620cc 643 pc.printf("MODE 1:ON\n");
soulx 14:7fe99764b2d0 644 serial_data=0;
soulx 13:218c22a620cc 645 break;
soulx 13:218c22a620cc 646 case '2':
soulx 13:218c22a620cc 647 leg_left_lower.SetMode(2);
soulx 13:218c22a620cc 648 leg_right_lower.SetMode(2);
soulx 13:218c22a620cc 649 pc.printf("MODE 2:ON\n");
soulx 14:7fe99764b2d0 650 serial_data=0;
soulx 14:7fe99764b2d0 651 break;
soulx 14:7fe99764b2d0 652 case '5':
soulx 14:7fe99764b2d0 653 wheel.reset();
soulx 14:7fe99764b2d0 654 serial_data=0;
soulx 14:7fe99764b2d0 655 break;
soulx 15:40ccb6433500 656
soulx 14:7fe99764b2d0 657 case '[':
soulx 15:40ccb6433500 658
soulx 14:7fe99764b2d0 659 test_limit();
soulx 13:218c22a620cc 660 break;
soulx 13:218c22a620cc 661 }
soulx 13:218c22a620cc 662
soulx 14:7fe99764b2d0 663 if(t.read() > 0.7f) {
soulx 13:218c22a620cc 664 t.reset();
soulx 13:218c22a620cc 665 pc.printf("left_up %d\n",left_up);
soulx 13:218c22a620cc 666 pc.printf("left_down %d\n",left_down);
soulx 13:218c22a620cc 667 pc.printf("right_up %d\n",right_up);
soulx 13:218c22a620cc 668 pc.printf("right_down %d\n",right_down);
soulx 13:218c22a620cc 669 pc.printf("offset_ll %d\n",offset_ll);
soulx 13:218c22a620cc 670 pc.printf("offset_rl %d\n",offset_rl);
soulx 14:7fe99764b2d0 671 pc.printf("Pulses is: %i\n", wheel.getPulses());
soulx 13:218c22a620cc 672 pc.printf("\n");
soulx 13:218c22a620cc 673
soulx 13:218c22a620cc 674 }
soulx 14:7fe99764b2d0 675 //serial_data=0;
soulx 13:218c22a620cc 676 state =0;
soulx 13:218c22a620cc 677
soulx 14:7fe99764b2d0 678 if(serial_data ==0) {
soulx 14:7fe99764b2d0 679 state += leg_left_upper.position_control(left_up);
soulx 14:7fe99764b2d0 680 state += leg_right_upper.position_control(right_up);
soulx 14:7fe99764b2d0 681 state += leg_left_lower.position_control(left_down);
soulx 14:7fe99764b2d0 682 state += leg_right_lower.position_control(right_down);
soulx 14:7fe99764b2d0 683 if(state == 8) {
soulx 14:7fe99764b2d0 684 myled=1;
soulx 14:7fe99764b2d0 685 } else {
soulx 14:7fe99764b2d0 686 myled=0;
soulx 14:7fe99764b2d0 687 }
soulx 13:218c22a620cc 688 }
soulx 13:218c22a620cc 689 }
soulx 14:7fe99764b2d0 690 }
soulx 14:7fe99764b2d0 691
soulx 14:7fe99764b2d0 692 void test_limit_motor(uint8_t dirction)
soulx 14:7fe99764b2d0 693 {
soulx 14:7fe99764b2d0 694 int state;
soulx 14:7fe99764b2d0 695 while(1) {
soulx 14:7fe99764b2d0 696 state = leg_left_upper.limit_motor(dirction);
soulx 14:7fe99764b2d0 697 pc.printf("state_lu %d\n",state);
soulx 14:7fe99764b2d0 698 state = leg_left_lower.limit_motor(dirction);
soulx 14:7fe99764b2d0 699 pc.printf("state_ll %d\n",state);
soulx 14:7fe99764b2d0 700 state = leg_right_upper.limit_motor(dirction);
soulx 14:7fe99764b2d0 701 pc.printf("state_ru %d\n",state);
soulx 14:7fe99764b2d0 702 state = leg_right_lower.limit_motor(dirction);
soulx 14:7fe99764b2d0 703 pc.printf("state_rl %d\n",state);
soulx 14:7fe99764b2d0 704 }
soulx 14:7fe99764b2d0 705 }
soulx 14:7fe99764b2d0 706
soulx 14:7fe99764b2d0 707 void test_sit()
soulx 14:7fe99764b2d0 708 {
soulx 14:7fe99764b2d0 709 uint16_t state=0;
soulx 15:40ccb6433500 710 // pc.printf("Test_sit\n");
soulx 14:7fe99764b2d0 711 //t.start();
soulx 14:7fe99764b2d0 712 // while(1) {
soulx 14:7fe99764b2d0 713 // sit
soulx 14:7fe99764b2d0 714 state += leg_left_upper.position_control(64);
soulx 14:7fe99764b2d0 715 state += leg_right_upper.position_control(64);
soulx 14:7fe99764b2d0 716 state += leg_left_lower.position_control(64);
soulx 14:7fe99764b2d0 717 state += leg_right_lower.position_control(64);
soulx 14:7fe99764b2d0 718 if(state == 8) {
soulx 14:7fe99764b2d0 719 myled=1;
soulx 14:7fe99764b2d0 720 } else {
soulx 14:7fe99764b2d0 721 myled=0;
soulx 14:7fe99764b2d0 722 }
soulx 14:7fe99764b2d0 723
soulx 14:7fe99764b2d0 724 // }
soulx 14:7fe99764b2d0 725 }
soulx 14:7fe99764b2d0 726
soulx 14:7fe99764b2d0 727 void test_stand()
soulx 14:7fe99764b2d0 728 {
soulx 14:7fe99764b2d0 729 uint16_t state=0;
soulx 14:7fe99764b2d0 730 // pc.printf("Test_stand\n");
soulx 14:7fe99764b2d0 731 // t.start();
soulx 14:7fe99764b2d0 732 // while(1) {
soulx 14:7fe99764b2d0 733
soulx 14:7fe99764b2d0 734 // stand
soulx 14:7fe99764b2d0 735 state += leg_left_upper.position_control(64);
soulx 14:7fe99764b2d0 736 state += leg_right_upper.position_control(64);
soulx 14:7fe99764b2d0 737 state += leg_left_lower.position_control(0);
soulx 14:7fe99764b2d0 738 state += leg_right_lower.position_control(0);
soulx 14:7fe99764b2d0 739 if(state == 8) {
soulx 14:7fe99764b2d0 740 myled=1;
soulx 14:7fe99764b2d0 741 } else {
soulx 14:7fe99764b2d0 742 myled=0;
soulx 14:7fe99764b2d0 743 }
soulx 14:7fe99764b2d0 744 // }
soulx 14:7fe99764b2d0 745 }
soulx 14:7fe99764b2d0 746
soulx 14:7fe99764b2d0 747 void test_sleep()
soulx 14:7fe99764b2d0 748 {
soulx 14:7fe99764b2d0 749 uint16_t state=0;
soulx 14:7fe99764b2d0 750 // pc.printf("Test_sleep\n");
soulx 14:7fe99764b2d0 751 // t.start();
soulx 14:7fe99764b2d0 752 // while(1) {
soulx 14:7fe99764b2d0 753
soulx 14:7fe99764b2d0 754 // sleep
soulx 14:7fe99764b2d0 755 state += leg_left_upper.position_control(0);
soulx 14:7fe99764b2d0 756 state += leg_right_upper.position_control(0);
soulx 14:7fe99764b2d0 757 state += leg_left_lower.position_control(64);
soulx 14:7fe99764b2d0 758 state += leg_right_lower.position_control(64);
soulx 14:7fe99764b2d0 759 if(state == 8) {
soulx 14:7fe99764b2d0 760 myled=1;
soulx 14:7fe99764b2d0 761 } else {
soulx 14:7fe99764b2d0 762 myled=0;
soulx 14:7fe99764b2d0 763 }
soulx 14:7fe99764b2d0 764 // }
soulx 15:40ccb6433500 765 }
soulx 15:40ccb6433500 766
soulx 15:40ccb6433500 767 void version()
soulx 15:40ccb6433500 768 {
soulx 15:40ccb6433500 769 int status_l=0,status_r=0;
soulx 15:40ccb6433500 770 int buff_l=0,buff_r=0;
soulx 15:40ccb6433500 771 int ticker_l=0,ticker_r=0;
soulx 15:40ccb6433500 772 int count_l=0,count_r=0;
soulx 15:40ccb6433500 773
soulx 15:40ccb6433500 774 uint16_t left_up=63;
soulx 15:40ccb6433500 775 uint16_t left_down=0;
soulx 15:40ccb6433500 776 uint16_t right_up=63;
soulx 15:40ccb6433500 777 uint16_t right_down=0;
soulx 15:40ccb6433500 778
soulx 15:40ccb6433500 779
soulx 15:40ccb6433500 780 t.start();
soulx 15:40ccb6433500 781
soulx 15:40ccb6433500 782 leg_left_lower.SetMode(0);
soulx 15:40ccb6433500 783 leg_right_lower.SetMode(0);
soulx 15:40ccb6433500 784 while(1) {
soulx 15:40ccb6433500 785 if(pc.readable() == 1) {
soulx 15:40ccb6433500 786 serial_data = pc.getc();
soulx 15:40ccb6433500 787 ticker_l =1;
soulx 15:40ccb6433500 788 ticker_r =1;
soulx 15:40ccb6433500 789 buff_l =0;
soulx 15:40ccb6433500 790 buff_r=0;
soulx 15:40ccb6433500 791 }
soulx 15:40ccb6433500 792 switch(serial_data) {
soulx 15:40ccb6433500 793 case 'q':
soulx 15:40ccb6433500 794 if(left_up >=63) {
soulx 15:40ccb6433500 795 left_up= 63;
soulx 15:40ccb6433500 796 } else {
soulx 15:40ccb6433500 797 left_up++;
soulx 15:40ccb6433500 798 }
soulx 15:40ccb6433500 799 serial_data=0;
soulx 15:40ccb6433500 800 break;
soulx 15:40ccb6433500 801
soulx 15:40ccb6433500 802 case 'a':
soulx 15:40ccb6433500 803 if(left_up ==0) {
soulx 15:40ccb6433500 804 left_up= 0;
soulx 15:40ccb6433500 805 } else {
soulx 15:40ccb6433500 806 left_up--;
soulx 15:40ccb6433500 807 }
soulx 15:40ccb6433500 808 serial_data=0;
soulx 15:40ccb6433500 809 break;
soulx 15:40ccb6433500 810
soulx 15:40ccb6433500 811 case 'e':
soulx 15:40ccb6433500 812 if(right_up >=63) {
soulx 15:40ccb6433500 813 right_up= 63;
soulx 15:40ccb6433500 814 } else {
soulx 15:40ccb6433500 815 right_up++;
soulx 15:40ccb6433500 816 }
soulx 15:40ccb6433500 817 serial_data=0;
soulx 15:40ccb6433500 818 break;
soulx 15:40ccb6433500 819
soulx 15:40ccb6433500 820 case 'd':
soulx 15:40ccb6433500 821 if(right_up ==0) {
soulx 15:40ccb6433500 822 right_up= 0;
soulx 15:40ccb6433500 823 } else {
soulx 15:40ccb6433500 824 right_up--;
soulx 15:40ccb6433500 825 }
soulx 15:40ccb6433500 826 serial_data=0;
soulx 15:40ccb6433500 827 break;
soulx 15:40ccb6433500 828
soulx 15:40ccb6433500 829 case 'n':
soulx 15:40ccb6433500 830 buff.left_up = left_up;
soulx 15:40ccb6433500 831 buff.left_down = left_down;
soulx 15:40ccb6433500 832 buff.right_up = right_up;
soulx 15:40ccb6433500 833 buff.right_down =right_down;
soulx 15:40ccb6433500 834 serial_data=0;
soulx 15:40ccb6433500 835 break;
soulx 15:40ccb6433500 836
soulx 15:40ccb6433500 837 case 'm':
soulx 15:40ccb6433500 838 buff2.left_up = left_up;
soulx 15:40ccb6433500 839 buff2.left_down = left_down;
soulx 15:40ccb6433500 840 buff2.right_up = right_up;
soulx 15:40ccb6433500 841 buff2.right_down =right_down;
soulx 15:40ccb6433500 842 serial_data=0;
soulx 15:40ccb6433500 843 break;
soulx 15:40ccb6433500 844
soulx 15:40ccb6433500 845 case 'w':
soulx 15:40ccb6433500 846 if(ticker_l == 1) {
soulx 15:40ccb6433500 847 status_l=leg_left_lower.limit_motor(1);
soulx 15:40ccb6433500 848 }
soulx 15:40ccb6433500 849
soulx 15:40ccb6433500 850 if(buff_l != 0 && status_l ==0) {
soulx 15:40ccb6433500 851 leg_left_lower.limit_motor(0);
soulx 15:40ccb6433500 852 ticker_l=0;
soulx 15:40ccb6433500 853 // serial_data=0;
soulx 15:40ccb6433500 854 }
soulx 15:40ccb6433500 855 buff_l = status_l;
soulx 15:40ccb6433500 856
soulx 15:40ccb6433500 857
soulx 15:40ccb6433500 858 if(ticker_r == 1) {
soulx 15:40ccb6433500 859 status_r=leg_right_lower.limit_motor(1);
soulx 15:40ccb6433500 860 }
soulx 15:40ccb6433500 861
soulx 15:40ccb6433500 862 if(buff_r != 0 && status_r ==0) {
soulx 15:40ccb6433500 863 leg_right_lower.limit_motor(0);
soulx 15:40ccb6433500 864 ticker_r =0;
soulx 15:40ccb6433500 865 // serial_data=0;
soulx 15:40ccb6433500 866 //buff_r=0;
soulx 15:40ccb6433500 867 }
soulx 15:40ccb6433500 868 buff_r = status_r;
soulx 15:40ccb6433500 869
soulx 15:40ccb6433500 870 leg_left_upper.position_control(buff.left_up);
soulx 15:40ccb6433500 871 leg_right_upper.position_control(buff.right_up);
soulx 15:40ccb6433500 872 // serial_data=0;
soulx 15:40ccb6433500 873 break;
soulx 15:40ccb6433500 874
soulx 15:40ccb6433500 875 case 's':
soulx 15:40ccb6433500 876 if(ticker_l ==1) {
soulx 15:40ccb6433500 877 status_l=leg_left_lower.limit_motor(2);
soulx 15:40ccb6433500 878 }
soulx 15:40ccb6433500 879
soulx 15:40ccb6433500 880 if(buff_l != 0 && status_l ==0) {
soulx 15:40ccb6433500 881 leg_left_lower.limit_motor(0);
soulx 15:40ccb6433500 882 ticker_l=0;
soulx 15:40ccb6433500 883 // serial_data=0;
soulx 15:40ccb6433500 884 // buff_l=0;
soulx 15:40ccb6433500 885 }
soulx 15:40ccb6433500 886 buff_l = status_l;
soulx 15:40ccb6433500 887
soulx 15:40ccb6433500 888 if(ticker_r == 1) {
soulx 15:40ccb6433500 889 status_r=leg_right_lower.limit_motor(2);
soulx 15:40ccb6433500 890 }
soulx 15:40ccb6433500 891
soulx 15:40ccb6433500 892 if(buff_r != 0 && status_r ==0) {
soulx 15:40ccb6433500 893 leg_right_lower.limit_motor(0);
soulx 15:40ccb6433500 894 ticker_r=0;
soulx 15:40ccb6433500 895 // serial_data=0;
soulx 15:40ccb6433500 896 //buff_r=0;
soulx 15:40ccb6433500 897 }
soulx 15:40ccb6433500 898 buff_r = status_r;
soulx 15:40ccb6433500 899 leg_left_upper.position_control(buff2.left_up);
soulx 15:40ccb6433500 900 leg_right_upper.position_control(buff2.right_up);
soulx 15:40ccb6433500 901 // serial_data=0;
soulx 15:40ccb6433500 902 break;
soulx 15:40ccb6433500 903 }
soulx 15:40ccb6433500 904
soulx 15:40ccb6433500 905 if(t.read() > 0.5f) {
soulx 15:40ccb6433500 906 t.reset();
soulx 15:40ccb6433500 907 // asdf = leg_left_lower.GetMode();
soulx 15:40ccb6433500 908 //pc.printf("ticker_l=%d\n",ticker_l);
soulx 15:40ccb6433500 909 //pc.printf("ticker_r=%d\n",ticker_r);
soulx 15:40ccb6433500 910 pc.printf("status_l=%d\n",status_l);
soulx 15:40ccb6433500 911 pc.printf("status_r=%d\n",status_r);
soulx 15:40ccb6433500 912 pc.printf("buff_l=%d\n",buff_l);
soulx 15:40ccb6433500 913 pc.printf("buff_r=%d\n",buff_r);
soulx 15:40ccb6433500 914 pc.printf("left_up %d\n",left_up);
soulx 15:40ccb6433500 915 pc.printf("left_down %d\n",left_down);
soulx 15:40ccb6433500 916 pc.printf("right_up %d\n",right_up);
soulx 15:40ccb6433500 917 pc.printf("right_down %d\n",right_down);
soulx 15:40ccb6433500 918
soulx 15:40ccb6433500 919 pc.printf("\n");
soulx 15:40ccb6433500 920
soulx 15:40ccb6433500 921 // asdf = leg_right_lower.GetMode();
soulx 15:40ccb6433500 922 // pc.printf("mode_right=%d\n",asdf);
soulx 15:40ccb6433500 923 }
soulx 15:40ccb6433500 924 if(serial_data ==0) {
soulx 15:40ccb6433500 925 leg_left_upper.position_control(left_up);
soulx 15:40ccb6433500 926 leg_right_upper.position_control(right_up);
soulx 15:40ccb6433500 927 }
soulx 15:40ccb6433500 928 }
soulx 6:8d80c84e0c09 929 }