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:
- 14:7fe99764b2d0
- Parent:
- 13:218c22a620cc
- Child:
- 15:40ccb6433500
- Child:
- 16:ec0cba3631bc
--- a/main.cpp Fri Jul 24 21:15:52 2015 +0000
+++ b/main.cpp Sat Jul 25 16:32:46 2015 +0000
@@ -8,6 +8,8 @@
#include "motor_relay.h"
#include "motion_control.h"
+#include "QEI.h"
+
//set frequancy unit in Hz
#define F_UPDATE 10.0f
#define TIMER_UPDATE 1.0f/F_UPDATE
@@ -15,6 +17,9 @@
//counter not receive from station
#define TIMEOUT_RESPONE_COMMAND 5
+//
+#define PULSE_RESOLUTION 500
+
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);
@@ -29,12 +34,18 @@
//serial for xbee
COMMUNICATION pan_a(TX_XBEE, RX_XBEE,115200,1000,1000); // tx, rx
+QEI wheel (ENCODE_A, ENCODE_B, ENCODE_Z, PULSE_RESOLUTION);
+
//Fuction prototye
void getCommand();
// init function
void calibration();
void test_position();
void test_status();
+void test_sleep();
+void test_sit();
+void test_stand();
+
void test_limit();
void test_motor();
@@ -76,26 +87,26 @@
pc.baud(115200);
pc.printf("Welcome to DOGWHEELSCHAIR\n");
- buff.left_up = 0;
- buff.right_up =0;
- buff.left_down = 64;
- buff.right_down = 64;
+ buff.left_up = 63;
+ buff.right_up =63;
+ buff.left_down = 63;
+ buff.right_down = 63;
if(mybutton == 0) {
calibration();
} else {
pc.printf("Lock position Min-Max...");
- leg_left_upper.SetMaxPosition(63787);
- leg_left_upper.SetMinPosition(28653);
+ leg_left_upper.SetMaxPosition(62787);
+ leg_left_upper.SetMinPosition(29353);
- leg_left_lower.SetMaxPosition(57609);
- leg_left_lower.SetMinPosition(17856);
+ leg_left_lower.SetMaxPosition(57000);
+ leg_left_lower.SetMinPosition(8200);
- leg_right_upper.SetMaxPosition(52094);
- leg_right_upper.SetMinPosition(18928);
+ leg_right_upper.SetMaxPosition(43494);
+ leg_right_upper.SetMinPosition(12028);
- leg_right_lower.SetMaxPosition(54383);
- leg_right_lower.SetMinPosition(8784);
+ leg_right_lower.SetMaxPosition(53383);
+ leg_right_lower.SetMinPosition(12584);
pc.printf("pass\n");
}
@@ -197,7 +208,7 @@
}
void test_status()
-{
+{
uint16_t state=0;
pc.printf("Test_status\n");
t.start();
@@ -298,7 +309,7 @@
{
pc.printf("TEST LIMIT ALL\n");
t.start();
- while(1) {
+// while(1) {
if(t.read() > 1.0f) {
t.reset();
pc.printf("leftUP_limit_up = ");
@@ -352,7 +363,8 @@
pc.printf("\n\n");
}
- }
+ // }
+ t.stop();
}
void test_motor()
@@ -390,13 +402,13 @@
void serial_control()
{
- uint16_t left_up=0;
+ uint16_t left_up=63;
uint16_t left_down=0;
- uint16_t right_up=0;
+ uint16_t right_up=63;
uint16_t right_down=0;
- uint16_t offset_ll=8700;
- uint16_t offset_rl=7500;
+ uint16_t offset_ll=11400;
+ uint16_t offset_rl=17800;
uint8_t state=0;
@@ -409,157 +421,171 @@
switch(serial_data) {
case 'q':
- if(left_up >=64) {
+ if(left_up >=63) {
left_up= 63;
} else {
left_up++;
}
- // serial_data=0;
+ serial_data=0;
break;
case 'a':
- if(left_up >=64 || left_up <=0) {
+ if(left_up >=63 || left_up <=0) {
left_up= 0;
} else {
left_up--;
}
- // serial_data=0;
+ serial_data=0;
break;
case 'z':
- if(left_up >=54) {
+ if(left_up >=53) {
left_up= 63;
} else {
left_up+=10;
}
- // serial_data=0;
+ serial_data=0;
break;
case 'Z':
- if(left_up >=54 || left_up <=10) {
+ if(left_up >=53 || left_up <=10) {
left_up= 0;
} else {
left_up-=10;
}
- // serial_data=0;
+ serial_data=0;
break;
case 'w':
- if(left_down >=64) {
+ if(left_down >=63) {
left_down= 63;
} else {
left_down++;
}
- // serial_data=0;
+ serial_data=0;
break;
case 's':
- if(left_down >=64 || left_down <=0) {
+ if(left_down >=63 || left_down <=0) {
left_down= 0;
} else {
left_down--;
}
- // serial_data=0;
+ serial_data=0;
break;
case 'x':
- if(left_down >=54) {
+ if(left_down >=53) {
left_down= 63;
} else {
left_down+=10;
}
- // serial_data=0;
+ serial_data=0;
break;
case 'X':
- if(left_down >=54 || left_down <=10) {
+ if(left_down >=53 || left_down <=10) {
left_down= 0;
} else {
left_down-=10;
}
-//serial_data=0;
+ serial_data=0;
break;
case 'e':
- if(right_up >=64) {
+ if(right_up >=63) {
right_up= 63;
} else {
right_up++;
}
- // serial_data=0;
+ serial_data=0;
break;
case 'd':
- if(right_up >=64 || right_up <=0) {
+ if(right_up >=63 || right_up <=0) {
right_up= 0;
} else {
right_up--;
}
- // serial_data=0;
+ serial_data=0;
break;
case 'c':
- if(right_up >=54) {
+ if(right_up >=53) {
right_up= 63;
} else {
right_up+=10;
}
- // serial_data=0;
+ serial_data=0;
break;
case 'C':
- if(right_up >=54 || right_up <=10) {
+ if(right_up >=53 || right_up <=10) {
right_up= 0;
} else {
right_up-=10;
}
- // serial_data=0;
+ serial_data=0;
break;
case 'r':
- if(right_down >=64) {
+ if(right_down >=63) {
right_down= 63;
} else {
right_down++;
}
-
+ serial_data=0;
break;
case 'f':
- if(right_down >=64 || right_down <=0) {
+ if(right_down >=63 || right_down <=0) {
right_down= 0;
} else {
right_down--;
}
-
+ serial_data=0;
+
break;
case 'v':
- if(right_down >=54) {
+ if(right_down >=53) {
right_down= 63;
} else {
right_down+=10;
}
-
+ serial_data=0;
+
break;
case 'V':
- if(right_down >=54 || right_down <=10) {
+ if(right_down >=53 || right_down <=10) {
right_down= 0;
} else {
right_down-=10;
}
-
+ serial_data=0;
break;
case 'p':
test_status();
break;
+ case '7':
+ test_sleep();
+ break;
+
+ case '8':
+ test_sit();
+ break;
+
+ case '9':
+ test_stand();
+ break;
+
case 'y':
if(offset_ll > 30000) {
offset_ll =30000;
@@ -568,6 +594,7 @@
offset_ll+=100;
}
leg_left_lower.SetOffset(offset_ll);
+ serial_data=0;
break;
case 'h':
@@ -578,6 +605,7 @@
offset_ll-=100;
}
leg_left_lower.SetOffset(offset_ll);
+ serial_data=0;
break;
case 'u':
@@ -588,6 +616,7 @@
offset_rl+=100;
}
leg_right_lower.SetOffset(offset_rl);
+ serial_data=0;
break;
case 'j':
@@ -598,26 +627,38 @@
offset_rl-=100;
}
leg_right_lower.SetOffset(offset_rl);
+ serial_data=0;
break;
- case '0':
+ case '0':
leg_left_lower.SetMode(0);
leg_right_lower.SetMode(0);
pc.printf("MODE 0:ON\n");
+ serial_data=0;
break;
case '1':
leg_left_lower.SetMode(1);
leg_right_lower.SetMode(1);
pc.printf("MODE 1:ON\n");
+ serial_data=0;
break;
case '2':
leg_left_lower.SetMode(2);
leg_right_lower.SetMode(2);
pc.printf("MODE 2:ON\n");
+ serial_data=0;
+ break;
+ case '5':
+ wheel.reset();
+ serial_data=0;
+ break;
+
+ case '[':
+ test_limit();
break;
}
- if(t.read() > 1.0f) {
+ if(t.read() > 0.7f) {
t.reset();
pc.printf("left_up %d\n",left_up);
pc.printf("left_down %d\n",left_down);
@@ -625,20 +666,98 @@
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("Pulses is: %i\n", wheel.getPulses());
pc.printf("\n");
}
- serial_data=0;
+ //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;
+ if(serial_data ==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;
+ }
}
}
+}
+
+void test_limit_motor(uint8_t dirction)
+{
+ int state;
+ while(1) {
+ state = leg_left_upper.limit_motor(dirction);
+ pc.printf("state_lu %d\n",state);
+ state = leg_left_lower.limit_motor(dirction);
+ pc.printf("state_ll %d\n",state);
+ state = leg_right_upper.limit_motor(dirction);
+ pc.printf("state_ru %d\n",state);
+ state = leg_right_lower.limit_motor(dirction);
+ pc.printf("state_rl %d\n",state);
+ }
+}
+
+void test_sit()
+{
+ uint16_t state=0;
+ // pc.printf("Test_sit\n");
+ //t.start();
+// while(1) {
+ // sit
+ state += leg_left_upper.position_control(64);
+ state += leg_right_upper.position_control(64);
+ state += leg_left_lower.position_control(64);
+ state += leg_right_lower.position_control(64);
+ if(state == 8) {
+ myled=1;
+ } else {
+ myled=0;
+ }
+
+// }
+}
+
+void test_stand()
+{
+ uint16_t state=0;
+// pc.printf("Test_stand\n");
+// t.start();
+// 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);
+ if(state == 8) {
+ myled=1;
+ } else {
+ myled=0;
+ }
+// }
+}
+
+void test_sleep()
+{
+ uint16_t state=0;
+// pc.printf("Test_sleep\n");
+// t.start();
+// while(1) {
+
+ // sleep
+ state += leg_left_upper.position_control(0);
+ state += leg_right_upper.position_control(0);
+ state += leg_left_lower.position_control(64);
+ state += leg_right_lower.position_control(64);
+ if(state == 8) {
+ myled=1;
+ } else {
+ myled=0;
+ }
+// }
}
\ No newline at end of file