BE@R lab
/
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
Diff: main.cpp
- Revision:
- 16:ec0cba3631bc
- Parent:
- 14:7fe99764b2d0
diff -r 7fe99764b2d0 -r ec0cba3631bc main.cpp
--- a/main.cpp Sat Jul 25 16:32:46 2015 +0000
+++ b/main.cpp Tue Oct 06 12:31:21 2015 +0000
@@ -20,6 +20,14 @@
//
#define PULSE_RESOLUTION 500
+//struct
+struct PARAM_WRITE {
+ uint16_t left_up;
+ uint16_t left_down;
+ uint16_t right_up;
+ uint16_t right_down;
+};
+
MOTION_CONTROL leg_left_upper(INA_L_U,INB_L_U,LIMIT_LU_U,LIMIT_LU_D,VR_LU);
MOTION_CONTROL leg_left_lower(INA_L_L,INB_L_L,LIMIT_LL_U,LIMIT_LL_D,VR_LL);
MOTION_CONTROL leg_right_upper(INA_R_U,INB_R_U,LIMIT_RU_U,LIMIT_RU_D,VR_RU);
@@ -42,9 +50,10 @@
void calibration();
void test_position();
void test_status();
+void test_status2(PARAM_WRITE d1, PARAM_WRITE d2);
void test_sleep();
void test_sit();
-void test_stand();
+void test_stand(PARAM_WRITE d1);
void test_limit();
void test_motor();
@@ -63,19 +72,13 @@
Timer t;
-//struct
-struct PARAM_WRITE {
- uint16_t left_up;
- uint16_t left_down;
- uint16_t right_up;
- uint16_t right_down;
-};
+
//variable
//volatile ANDANTE_PROTOCOL_PACKET *param;
volatile uint8_t status=0;
//volatile PARAM_WRITE buff;
-PARAM_WRITE buff;
+PARAM_WRITE buff1,buff2;
int serial_data;
@@ -87,10 +90,6 @@
pc.baud(115200);
pc.printf("Welcome to DOGWHEELSCHAIR\n");
- buff.left_up = 63;
- buff.right_up =63;
- buff.left_down = 63;
- buff.right_down = 63;
if(mybutton == 0) {
calibration();
@@ -254,10 +253,10 @@
} else if(status == 2) {
state =0;
// stand
- state += leg_left_upper.position_control(64);
- state += leg_right_upper.position_control(64);
- state += leg_left_lower.position_control(0);
- state += leg_right_lower.position_control(0);
+ state += leg_left_upper.position_control(63);
+ state += leg_right_upper.position_control(56);
+ state += leg_left_lower.position_control(7);
+ state += leg_right_lower.position_control(10);
if(state == 8) {
myled=1;
} else {
@@ -272,16 +271,16 @@
{
switch(packet->instructionErrorId) {
case 0x01:
- buff.left_up = Utilities::ConvertUInt8ArrayToInt16(packet->parameter);
+ buff1.left_up = Utilities::ConvertUInt8ArrayToInt16(packet->parameter);
break;
case 0x02:
- buff.left_down = Utilities::ConvertUInt8ArrayToInt16(packet->parameter);
+ buff1.left_down = Utilities::ConvertUInt8ArrayToInt16(packet->parameter);
break;
case 0x03:
- buff.right_up = Utilities::ConvertUInt8ArrayToInt16(packet->parameter);
+ buff1.right_up = Utilities::ConvertUInt8ArrayToInt16(packet->parameter);
break;
case 0x04:
- buff.right_down = Utilities::ConvertUInt8ArrayToInt16(packet->parameter);
+ buff1.right_down = Utilities::ConvertUInt8ArrayToInt16(packet->parameter);
break;
}
}
@@ -293,10 +292,10 @@
while(1) {
state =0;
// sit
- state += leg_left_upper.position_control(buff.left_up);
- state += leg_right_upper.position_control(buff.right_up);
- state += leg_left_lower.position_control(buff.left_down);
- state += leg_right_lower.position_control(buff.right_down);
+ state += leg_left_upper.position_control(buff1.left_up);
+ state += leg_right_upper.position_control(buff1.right_up);
+ state += leg_left_lower.position_control(buff1.left_down);
+ state += leg_right_lower.position_control(buff1.right_down);
if(state == 8) {
myled=1;
} else {
@@ -308,8 +307,8 @@
void test_limit()
{
pc.printf("TEST LIMIT ALL\n");
- t.start();
-// while(1) {
+ t.start();
+ while(1) {
if(t.read() > 1.0f) {
t.reset();
pc.printf("leftUP_limit_up = ");
@@ -363,8 +362,8 @@
pc.printf("\n\n");
}
- // }
- t.stop();
+ }
+ //t.stop();
}
void test_motor()
@@ -430,7 +429,7 @@
break;
case 'a':
- if(left_up >=63 || left_up <=0) {
+ if(left_up ==0) {
left_up= 0;
} else {
left_up--;
@@ -448,7 +447,7 @@
break;
case 'Z':
- if(left_up >=53 || left_up <=10) {
+ if(left_up <=10) {
left_up= 0;
} else {
left_up-=10;
@@ -466,7 +465,7 @@
break;
case 's':
- if(left_down >=63 || left_down <=0) {
+ if(left_down ==0) {
left_down= 0;
} else {
left_down--;
@@ -484,7 +483,7 @@
break;
case 'X':
- if(left_down >=53 || left_down <=10) {
+ if(left_down <=10) {
left_down= 0;
} else {
left_down-=10;
@@ -503,7 +502,7 @@
break;
case 'd':
- if(right_up >=63 || right_up <=0) {
+ if(right_up ==0) {
right_up= 0;
} else {
right_up--;
@@ -521,7 +520,7 @@
break;
case 'C':
- if(right_up >=53 || right_up <=10) {
+ if(right_up <=10) {
right_up= 0;
} else {
right_up-=10;
@@ -542,7 +541,7 @@
break;
case 'f':
- if(right_down >=63 || right_down <=0) {
+ if(right_down <=0) {
right_down= 0;
} else {
right_down--;
@@ -562,7 +561,7 @@
break;
case 'V':
- if(right_down >=53 || right_down <=10) {
+ if(right_down <=10) {
right_down= 0;
} else {
right_down-=10;
@@ -583,7 +582,7 @@
break;
case '9':
- test_stand();
+ test_stand(buff1);
break;
case 'y':
@@ -656,6 +655,25 @@
case '[':
test_limit();
break;
+
+ case 'n':
+ buff1.left_up = left_up;
+ buff1.left_down = left_down;
+ buff1.right_up = right_up;
+ buff1.right_down =right_down;
+ break;
+
+ case 'm':
+ buff2.left_up = left_up;
+ buff2.left_down = left_down;
+ buff2.right_up = right_up;
+ buff2.right_down =right_down;
+ break;
+
+ case 'b':
+ test_status2(buff1,buff2);
+ break;
+
}
if(t.read() > 0.7f) {
@@ -722,24 +740,24 @@
// }
}
-void test_stand()
+void test_stand(PARAM_WRITE d1)
{
uint16_t state=0;
// pc.printf("Test_stand\n");
// t.start();
-// while(1) {
+ while(1) {
// stand
- state += leg_left_upper.position_control(64);
- state += leg_right_upper.position_control(64);
- state += leg_left_lower.position_control(0);
- state += leg_right_lower.position_control(0);
+ state += leg_left_upper.position_control(d1.left_up);
+ state += leg_right_upper.position_control(d1.right_up);
+ state += leg_left_lower.position_control(d1.left_down);
+ state += leg_right_lower.position_control(d1.right_down);
if(state == 8) {
myled=1;
} else {
myled=0;
}
-// }
+ }
}
void test_sleep()
@@ -760,4 +778,53 @@
myled=0;
}
// }
+}
+
+void test_status2(PARAM_WRITE d1, PARAM_WRITE d2)
+{
+ uint16_t state=0;
+ pc.printf("Test_status\n");
+ t.start();
+ while(1) {
+
+ if(pc.readable() == 1) {
+ serial_data = pc.getc();
+ }
+
+ if(t.read() > 30.0f) {
+ t.reset();
+ if(status >=1) {
+ status =0;
+ } else {
+ status++;
+ }
+ }
+
+ if(status == 0) {
+ state =0;
+ // sleep
+ state += leg_left_upper.position_control(d1.left_up);
+ state += leg_right_upper.position_control(d1.right_up);
+ state += leg_left_lower.position_control(d1.left_down);
+ state += leg_right_lower.position_control(d1.right_down);
+ if(state == 8) {
+ myled=1;
+ } else {
+ myled=0;
+ }
+ } else if(status == 1) {
+ state =0;
+ // sit
+ state += leg_left_upper.position_control(d2.left_up);
+ state += leg_right_upper.position_control(d2.right_up);
+ state += leg_left_lower.position_control(d2.left_down);
+ state += leg_right_lower.position_control(d2.right_down);
+ if(state == 8) {
+ myled=1;
+ } else {
+ myled=0;
+ }
+ }
+ }
+ //t.stop();
}
\ No newline at end of file