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:
- 12:2564eac22e0a
- Parent:
- 11:336dd293daa1
- Child:
- 13:218c22a620cc
--- a/main.cpp Sun Jul 19 17:38:38 2015 +0000
+++ b/main.cpp Mon Jul 20 09:15:40 2015 +0000
@@ -34,7 +34,13 @@
// init function
void calibration();
void test_position();
+void test_status();
+void test_limit();
+void test_motor();
+void routine();
+
+void management(ANDANTE_PROTOCOL_PACKET *packet);
void copy_data(ANDANTE_PROTOCOL_PACKET *scr, ANDANTE_PROTOCOL_PACKET *dst);
@@ -44,16 +50,33 @@
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 ANDANTE_PROTOCOL_PACKET *param;
volatile uint8_t status=0;
+//volatile PARAM_WRITE buff;
+PARAM_WRITE buff;
//Main function
int main()
{
- int state=0;
+ //int state=0;
+
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;
+
if(mybutton == 0) {
calibration();
} else {
@@ -74,58 +97,10 @@
Update_command.attach(&getCommand,TIMER_UPDATE);
- t.start();
- while(1) {
-
-
- if(t.read() > 10.0f) {
- t.reset();
- if(status >3) {
- status =0;
- } else {
- status++;
- }
- }
-
- if(status == 0) {
- state =0;
- // 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;
- }
- } else if(status == 1 || status ==3) {
- state =0;
- // 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;
- }
- } 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);
- if(state == 8) {
- myled=1;
- } else {
- myled=0;
- }
- }
- }
-
+ test_motor();
+ test_limit();
+ test_status();
+ routine();
}
void getCommand()
@@ -145,11 +120,12 @@
}
- // pan_a.sendCommunicatePacket(&packet);
+ //pan_a.sendCommunicatePacket(&packet);
//update command
//setControl(&packet);
+
} else if(status == ANDANTE_ERRBIT_READ_TIMEOUT) {
count++;
}
@@ -213,4 +189,187 @@
state=0;
} while(state != 2);
pc.printf("[Finish test]\n");
+}
+
+void test_status()
+{
+ uint16_t state=0;
+ t.start();
+ while(1) {
+
+
+ if(t.read() > 10.0f) {
+ t.reset();
+ if(status >3) {
+ status =0;
+ } else {
+ status++;
+ }
+ }
+
+ if(status == 0) {
+ state =0;
+ // 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;
+ }
+ } else if(status == 1 || status ==3) {
+ state =0;
+ // 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;
+ }
+ } 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);
+ if(state == 8) {
+ myled=1;
+ } else {
+ myled=0;
+ }
+ }
+ }
+ //t.stop();
+}
+
+void management(ANDANTE_PROTOCOL_PACKET *packet)
+{
+ switch(packet->instructionErrorId) {
+ case 0x01:
+ buff.left_up = Utilities::ConvertUInt8ArrayToInt16(packet->parameter);
+ break;
+ case 0x02:
+ buff.left_down = Utilities::ConvertUInt8ArrayToInt16(packet->parameter);
+ break;
+ case 0x03:
+ buff.right_up = Utilities::ConvertUInt8ArrayToInt16(packet->parameter);
+ break;
+ case 0x04:
+ buff.right_down = Utilities::ConvertUInt8ArrayToInt16(packet->parameter);
+ break;
+ }
+}
+
+void routine()
+{
+ uint8_t state=0;
+// PARAM_WRITE pos = buff;
+ 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);
+ if(state == 8) {
+ myled=1;
+ } else {
+ myled=0;
+ }
+ }
+}
+
+void test_limit()
+{
+ pc.printf("TEST LIMIT ALL\n");
+ while(1) {
+ pc.printf("leftUP_limit_up = ");
+ if(leg_left_upper.GetLimitUp() == 0)
+ 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("leftLOW_limit_up = ");
+ if(leg_left_lower.GetLimitUp() == 0)
+ 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("rightUP_limit_up = ");
+ if(leg_right_upper.GetLimitUp() == 0)
+ 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("rightLOW_limit_up = ");
+ if(leg_right_lower.GetLimitUp() == 0)
+ 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("\n\n");
+ }
+}
+
+void test_motor()
+{
+ uint8_t state=0;
+
+ pc.printf("TEST MOTOR DELAY\n");
+ t.start();
+ while(1) {
+ if(t.read() > 1.5f) {
+ t.reset();
+ if(state >1) {
+ state =0;
+ } else {
+ state++;
+ }
+ }
+
+ if(state ==0) {
+ leg_left_upper.limit_motor(1);
+ leg_right_upper.limit_motor(1);
+ leg_left_lower.limit_motor(1);
+ leg_right_lower.limit_motor(1);
+ } else {
+ {
+ leg_left_upper.limit_motor(2);
+ leg_right_upper.limit_motor(2);
+ leg_left_lower.limit_motor(2);
+ leg_right_lower.limit_motor(2);
+ }
+ }
+ }
}
\ No newline at end of file