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:
- 15:40ccb6433500
- Parent:
- 14:7fe99764b2d0
--- a/main.cpp Sat Jul 25 16:32:46 2015 +0000
+++ b/main.cpp Mon Dec 07 09:16:24 2015 +0000
@@ -57,7 +57,7 @@
void copy_data(ANDANTE_PROTOCOL_PACKET *scr, ANDANTE_PROTOCOL_PACKET *dst);
-
+void version();
//set foreground
Ticker Update_command;
@@ -75,7 +75,7 @@
//volatile ANDANTE_PROTOCOL_PACKET *param;
volatile uint8_t status=0;
//volatile PARAM_WRITE buff;
-PARAM_WRITE buff;
+PARAM_WRITE buff,buff2;
int serial_data;
@@ -96,22 +96,23 @@
calibration();
} else {
pc.printf("Lock position Min-Max...");
- leg_left_upper.SetMaxPosition(62787);
- leg_left_upper.SetMinPosition(29353);
+ leg_left_upper.SetMaxPosition(62463);
+ leg_left_upper.SetMinPosition(29319);
- leg_left_lower.SetMaxPosition(57000);
- leg_left_lower.SetMinPosition(8200);
+ leg_left_lower.SetMaxPosition(43450);
+ leg_left_lower.SetMinPosition(288);
- leg_right_upper.SetMaxPosition(43494);
- leg_right_upper.SetMinPosition(12028);
+ leg_right_upper.SetMaxPosition(42794);
+ leg_right_upper.SetMinPosition(11282);
- leg_right_lower.SetMaxPosition(53383);
- leg_right_lower.SetMinPosition(12584);
+ leg_right_lower.SetMaxPosition(37001);
+ leg_right_lower.SetMinPosition(144);
pc.printf("pass\n");
}
Update_command.attach(&getCommand,TIMER_UPDATE);
+ version();
serial_control();
//test_motor();
//test_limit();
@@ -308,63 +309,63 @@
void test_limit()
{
pc.printf("TEST LIMIT ALL\n");
- t.start();
+
// while(1) {
- 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");
+ 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() == 1)
- 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() == 1)
- 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() == 1)
- 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() == 1)
- 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() == 1)
- 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() == 1)
- 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() == 1)
- 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");
- }
- // }
- t.stop();
+ pc.printf("\n\n");
+ }
+// }
+ t.stop();
}
void test_motor()
@@ -652,8 +653,9 @@
wheel.reset();
serial_data=0;
break;
-
+
case '[':
+
test_limit();
break;
}
@@ -705,7 +707,7 @@
void test_sit()
{
uint16_t state=0;
- // pc.printf("Test_sit\n");
+// pc.printf("Test_sit\n");
//t.start();
// while(1) {
// sit
@@ -760,4 +762,168 @@
myled=0;
}
// }
+}
+
+void version()
+{
+ int status_l=0,status_r=0;
+ int buff_l=0,buff_r=0;
+ int ticker_l=0,ticker_r=0;
+ int count_l=0,count_r=0;
+
+ uint16_t left_up=63;
+ uint16_t left_down=0;
+ uint16_t right_up=63;
+ uint16_t right_down=0;
+
+
+ t.start();
+
+ leg_left_lower.SetMode(0);
+ leg_right_lower.SetMode(0);
+ while(1) {
+ if(pc.readable() == 1) {
+ serial_data = pc.getc();
+ ticker_l =1;
+ ticker_r =1;
+ buff_l =0;
+ buff_r=0;
+ }
+ switch(serial_data) {
+ case 'q':
+ if(left_up >=63) {
+ left_up= 63;
+ } else {
+ left_up++;
+ }
+ serial_data=0;
+ break;
+
+ case 'a':
+ if(left_up ==0) {
+ left_up= 0;
+ } else {
+ left_up--;
+ }
+ serial_data=0;
+ break;
+
+ case 'e':
+ if(right_up >=63) {
+ right_up= 63;
+ } else {
+ right_up++;
+ }
+ serial_data=0;
+ break;
+
+ case 'd':
+ if(right_up ==0) {
+ right_up= 0;
+ } else {
+ right_up--;
+ }
+ serial_data=0;
+ break;
+
+ case 'n':
+ buff.left_up = left_up;
+ buff.left_down = left_down;
+ buff.right_up = right_up;
+ buff.right_down =right_down;
+ serial_data=0;
+ break;
+
+ case 'm':
+ buff2.left_up = left_up;
+ buff2.left_down = left_down;
+ buff2.right_up = right_up;
+ buff2.right_down =right_down;
+ serial_data=0;
+ break;
+
+ case 'w':
+ if(ticker_l == 1) {
+ status_l=leg_left_lower.limit_motor(1);
+ }
+
+ if(buff_l != 0 && status_l ==0) {
+ leg_left_lower.limit_motor(0);
+ ticker_l=0;
+ // serial_data=0;
+ }
+ buff_l = status_l;
+
+
+ if(ticker_r == 1) {
+ status_r=leg_right_lower.limit_motor(1);
+ }
+
+ if(buff_r != 0 && status_r ==0) {
+ leg_right_lower.limit_motor(0);
+ ticker_r =0;
+ // serial_data=0;
+ //buff_r=0;
+ }
+ buff_r = status_r;
+
+ leg_left_upper.position_control(buff.left_up);
+ leg_right_upper.position_control(buff.right_up);
+ // serial_data=0;
+ break;
+
+ case 's':
+ if(ticker_l ==1) {
+ status_l=leg_left_lower.limit_motor(2);
+ }
+
+ if(buff_l != 0 && status_l ==0) {
+ leg_left_lower.limit_motor(0);
+ ticker_l=0;
+ // serial_data=0;
+ // buff_l=0;
+ }
+ buff_l = status_l;
+
+ if(ticker_r == 1) {
+ status_r=leg_right_lower.limit_motor(2);
+ }
+
+ if(buff_r != 0 && status_r ==0) {
+ leg_right_lower.limit_motor(0);
+ ticker_r=0;
+ // serial_data=0;
+ //buff_r=0;
+ }
+ buff_r = status_r;
+ leg_left_upper.position_control(buff2.left_up);
+ leg_right_upper.position_control(buff2.right_up);
+ // serial_data=0;
+ break;
+ }
+
+ if(t.read() > 0.5f) {
+ t.reset();
+ // asdf = leg_left_lower.GetMode();
+ //pc.printf("ticker_l=%d\n",ticker_l);
+ //pc.printf("ticker_r=%d\n",ticker_r);
+ pc.printf("status_l=%d\n",status_l);
+ pc.printf("status_r=%d\n",status_r);
+ pc.printf("buff_l=%d\n",buff_l);
+ pc.printf("buff_r=%d\n",buff_r);
+ 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("\n");
+
+ // asdf = leg_right_lower.GetMode();
+ // pc.printf("mode_right=%d\n",asdf);
+ }
+ if(serial_data ==0) {
+ leg_left_upper.position_control(left_up);
+ leg_right_upper.position_control(right_up);
+ }
+ }
}
\ No newline at end of file