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:
- 13:218c22a620cc
- Parent:
- 12:2564eac22e0a
- Child:
- 14:7fe99764b2d0
--- a/main.cpp Mon Jul 20 09:15:40 2015 +0000
+++ b/main.cpp Fri Jul 24 21:15:52 2015 +0000
@@ -25,7 +25,7 @@
//communication config
//serial for debug
-iSerial pc(USBTX, USBRX);
+Serial pc(USBTX, USBRX);
//serial for xbee
COMMUNICATION pan_a(TX_XBEE, RX_XBEE,115200,1000,1000); // tx, rx
@@ -40,6 +40,8 @@
void routine();
+void serial_control();
+
void management(ANDANTE_PROTOCOL_PACKET *packet);
void copy_data(ANDANTE_PROTOCOL_PACKET *scr, ANDANTE_PROTOCOL_PACKET *dst);
@@ -64,6 +66,8 @@
//volatile PARAM_WRITE buff;
PARAM_WRITE buff;
+int serial_data;
+
//Main function
int main()
{
@@ -81,24 +85,25 @@
calibration();
} else {
pc.printf("Lock position Min-Max...");
- leg_left_upper.SetMaxPosition(56244);
- leg_left_upper.SetMinPosition(20806);
+ leg_left_upper.SetMaxPosition(63787);
+ leg_left_upper.SetMinPosition(28653);
- leg_left_lower.SetMaxPosition(50996);
- leg_left_lower.SetMinPosition(5371);
+ leg_left_lower.SetMaxPosition(57609);
+ leg_left_lower.SetMinPosition(17856);
- leg_right_upper.SetMaxPosition(38985);
- leg_right_upper.SetMinPosition(8545);
+ leg_right_upper.SetMaxPosition(52094);
+ leg_right_upper.SetMinPosition(18928);
- leg_right_lower.SetMaxPosition(38027);
- leg_right_lower.SetMinPosition(40);
+ leg_right_lower.SetMaxPosition(54383);
+ leg_right_lower.SetMinPosition(8784);
pc.printf("pass\n");
}
Update_command.attach(&getCommand,TIMER_UPDATE);
- test_motor();
- test_limit();
+ serial_control();
+ //test_motor();
+ //test_limit();
test_status();
routine();
}
@@ -192,11 +197,15 @@
}
void test_status()
-{
+{
uint16_t state=0;
+ pc.printf("Test_status\n");
t.start();
while(1) {
+ if(pc.readable() == 1) {
+ serial_data = pc.getc();
+ }
if(t.read() > 10.0f) {
t.reset();
@@ -288,57 +297,61 @@
void test_limit()
{
pc.printf("TEST LIMIT ALL\n");
+ t.start();
while(1) {
- pc.printf("leftUP_limit_up = ");
- if(leg_left_upper.GetLimitUp() == 0)
- pc.printf("shot\n");
- else
- pc.printf("open\n");
+ if(t.read() > 1.0f) {
+ t.reset();
+ pc.printf("leftUP_limit_up = ");
+ if(leg_left_upper.GetLimitUp() == 1)
+ pc.printf("shot\n");
+ else
+ pc.printf("open\n");
- pc.printf("leftUP_limit_down = ");
- if(leg_left_upper.GetLimitDown() == 0)
- pc.printf("shot\n");
- else
- pc.printf("open\n");
+ pc.printf("leftUP_limit_down = ");
+ if(leg_left_upper.GetLimitDown() == 1)
+ pc.printf("shot\n");
+ else
+ pc.printf("open\n");
- pc.printf("leftLOW_limit_up = ");
- if(leg_left_lower.GetLimitUp() == 0)
- pc.printf("shot\n");
- else
- pc.printf("open\n");
+ pc.printf("leftLOW_limit_up = ");
+ if(leg_left_lower.GetLimitUp() == 1)
+ pc.printf("shot\n");
+ else
+ pc.printf("open\n");
- pc.printf("leftLow_limit_down = ");
- if(leg_left_lower.GetLimitDown() == 0)
- pc.printf("shot\n");
- else
- pc.printf("open\n");
+ pc.printf("leftLow_limit_down = ");
+ if(leg_left_lower.GetLimitDown() == 1)
+ pc.printf("shot\n");
+ else
+ pc.printf("open\n");
///////////////////////////////////////////////////////////////
- pc.printf("rightUP_limit_up = ");
- if(leg_right_upper.GetLimitUp() == 0)
- pc.printf("shot\n");
- else
- pc.printf("open\n");
+ pc.printf("rightUP_limit_up = ");
+ if(leg_right_upper.GetLimitUp() == 1)
+ pc.printf("shot\n");
+ else
+ pc.printf("open\n");
- pc.printf("rightUP_limit_down = ");
- if(leg_right_upper.GetLimitDown() == 0)
- pc.printf("shot\n");
- else
- pc.printf("open\n");
+ pc.printf("rightUP_limit_down = ");
+ if(leg_right_upper.GetLimitDown() == 1)
+ pc.printf("shot\n");
+ else
+ pc.printf("open\n");
- pc.printf("rightLOW_limit_up = ");
- if(leg_right_lower.GetLimitUp() == 0)
- pc.printf("shot\n");
- else
- pc.printf("open\n");
+ pc.printf("rightLOW_limit_up = ");
+ if(leg_right_lower.GetLimitUp() == 1)
+ pc.printf("shot\n");
+ else
+ pc.printf("open\n");
- pc.printf("rightLow_limit_down = ");
- if(leg_right_lower.GetLimitDown() == 0)
- pc.printf("shot\n");
- else
- pc.printf("open\n");
+ pc.printf("rightLow_limit_down = ");
+ if(leg_right_lower.GetLimitDown() == 1)
+ pc.printf("shot\n");
+ else
+ pc.printf("open\n");
- pc.printf("\n\n");
+ pc.printf("\n\n");
+ }
}
}
@@ -372,4 +385,260 @@
}
}
}
+}
+
+void serial_control()
+{
+
+ uint16_t left_up=0;
+ uint16_t left_down=0;
+ uint16_t right_up=0;
+ uint16_t right_down=0;
+
+ uint16_t offset_ll=8700;
+ uint16_t offset_rl=7500;
+
+ uint8_t state=0;
+
+ t.start();
+
+ while(1) {
+ if(pc.readable() == 1) {
+ serial_data = pc.getc();
+ }
+
+ switch(serial_data) {
+ case 'q':
+ if(left_up >=64) {
+ left_up= 63;
+ } else {
+ left_up++;
+ }
+ // serial_data=0;
+ break;
+
+ case 'a':
+ if(left_up >=64 || left_up <=0) {
+ left_up= 0;
+ } else {
+ left_up--;
+ }
+ // serial_data=0;
+ break;
+
+ case 'z':
+ if(left_up >=54) {
+ left_up= 63;
+ } else {
+ left_up+=10;
+ }
+ // serial_data=0;
+ break;
+
+ case 'Z':
+ if(left_up >=54 || left_up <=10) {
+ left_up= 0;
+ } else {
+ left_up-=10;
+ }
+ // serial_data=0;
+ break;
+
+ case 'w':
+ if(left_down >=64) {
+ left_down= 63;
+ } else {
+ left_down++;
+ }
+ // serial_data=0;
+ break;
+
+ case 's':
+ if(left_down >=64 || left_down <=0) {
+ left_down= 0;
+ } else {
+ left_down--;
+ }
+ // serial_data=0;
+ break;
+
+ case 'x':
+ if(left_down >=54) {
+ left_down= 63;
+ } else {
+ left_down+=10;
+ }
+ // serial_data=0;
+ break;
+
+ case 'X':
+ if(left_down >=54 || left_down <=10) {
+ left_down= 0;
+ } else {
+ left_down-=10;
+ }
+//serial_data=0;
+ break;
+
+
+ case 'e':
+ if(right_up >=64) {
+ right_up= 63;
+ } else {
+ right_up++;
+ }
+ // serial_data=0;
+ break;
+
+ case 'd':
+ if(right_up >=64 || right_up <=0) {
+ right_up= 0;
+ } else {
+ right_up--;
+ }
+ // serial_data=0;
+ break;
+
+ case 'c':
+ if(right_up >=54) {
+ right_up= 63;
+ } else {
+ right_up+=10;
+ }
+ // serial_data=0;
+ break;
+
+ case 'C':
+ if(right_up >=54 || right_up <=10) {
+ right_up= 0;
+ } else {
+ right_up-=10;
+ }
+ // serial_data=0;
+ break;
+
+
+
+
+ case 'r':
+ if(right_down >=64) {
+ right_down= 63;
+ } else {
+ right_down++;
+ }
+
+ break;
+
+ case 'f':
+ if(right_down >=64 || right_down <=0) {
+ right_down= 0;
+ } else {
+ right_down--;
+ }
+
+ break;
+
+ case 'v':
+ if(right_down >=54) {
+ right_down= 63;
+ } else {
+ right_down+=10;
+ }
+
+ break;
+
+ case 'V':
+ if(right_down >=54 || right_down <=10) {
+ right_down= 0;
+ } else {
+ right_down-=10;
+ }
+
+ break;
+
+ case 'p':
+ test_status();
+ break;
+
+ case 'y':
+ if(offset_ll > 30000) {
+ offset_ll =30000;
+ } else {
+
+ offset_ll+=100;
+ }
+ leg_left_lower.SetOffset(offset_ll);
+ break;
+
+ case 'h':
+ if(offset_ll <= 100) {
+ offset_ll =0;
+ } else {
+
+ offset_ll-=100;
+ }
+ leg_left_lower.SetOffset(offset_ll);
+ break;
+
+ case 'u':
+ if(offset_rl > 30000) {
+ offset_rl =30000;
+ } else {
+
+ offset_rl+=100;
+ }
+ leg_right_lower.SetOffset(offset_rl);
+ break;
+
+ case 'j':
+ if(offset_rl <= 100) {
+ offset_rl =0;
+ } else {
+
+ offset_rl-=100;
+ }
+ leg_right_lower.SetOffset(offset_rl);
+ break;
+
+ case '0':
+ leg_left_lower.SetMode(0);
+ leg_right_lower.SetMode(0);
+ pc.printf("MODE 0:ON\n");
+ break;
+ case '1':
+ leg_left_lower.SetMode(1);
+ leg_right_lower.SetMode(1);
+ pc.printf("MODE 1:ON\n");
+ break;
+ case '2':
+ leg_left_lower.SetMode(2);
+ leg_right_lower.SetMode(2);
+ pc.printf("MODE 2:ON\n");
+ break;
+ }
+
+ if(t.read() > 1.0f) {
+ t.reset();
+ pc.printf("left_up %d\n",left_up);
+ pc.printf("left_down %d\n",left_down);
+ pc.printf("right_up %d\n",right_up);
+ pc.printf("right_down %d\n",right_down);
+ pc.printf("offset_ll %d\n",offset_ll);
+ pc.printf("offset_rl %d\n",offset_rl);
+ pc.printf("\n");
+
+ }
+ serial_data=0;
+ state =0;
+
+ state += leg_left_upper.position_control(left_up);
+ state += leg_right_upper.position_control(right_up);
+ state += leg_left_lower.position_control(left_down);
+ state += leg_right_lower.position_control(right_down);
+ if(state == 8) {
+ myled=1;
+ } else {
+ myled=0;
+ }
+ }
}
\ No newline at end of file