BE@R lab / Mbed 2 deprecated 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

main.cpp@11:336dd293daa1, 2015-07-19 (annotated)

Committer:
soulx
Date:
Sun Jul 19 17:38:38 2015 +0000
Revision:
11:336dd293daa1
Parent:
10:53cb691e22bf
Child:
12:2564eac22e0a
pre-test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
soulx 0:2433ddae2772 1 #include "mbed.h"
soulx 0:2433ddae2772 2 #include "pin_config.h"
soulx 0:2433ddae2772 3
soulx 6:8d80c84e0c09 4 #include "communication.h"
soulx 6:8d80c84e0c09 5 #include "protocol.h"
soulx 11:336dd293daa1 6 #include "iSerial.h"
soulx 6:8d80c84e0c09 7
soulx 10:53cb691e22bf 8 #include "motor_relay.h"
soulx 10:53cb691e22bf 9 #include "motion_control.h"
soulx 10:53cb691e22bf 10
soulx 6:8d80c84e0c09 11 //set frequancy unit in Hz
soulx 6:8d80c84e0c09 12 #define F_UPDATE 10.0f
soulx 6:8d80c84e0c09 13 #define TIMER_UPDATE 1.0f/F_UPDATE
soulx 6:8d80c84e0c09 14
soulx 6:8d80c84e0c09 15 //counter not receive from station
soulx 6:8d80c84e0c09 16 #define TIMEOUT_RESPONE_COMMAND 5
soulx 10:53cb691e22bf 17
soulx 10:53cb691e22bf 18 MOTION_CONTROL leg_left_upper(INA_L_U,INB_L_U,LIMIT_LU_U,LIMIT_LU_D,VR_LU);
soulx 10:53cb691e22bf 19 MOTION_CONTROL leg_left_lower(INA_L_L,INB_L_L,LIMIT_LL_U,LIMIT_LL_D,VR_LL);
soulx 10:53cb691e22bf 20 MOTION_CONTROL leg_right_upper(INA_R_U,INB_R_U,LIMIT_RU_U,LIMIT_RU_D,VR_RU);
soulx 10:53cb691e22bf 21 MOTION_CONTROL leg_right_lower(INA_R_L,INB_R_L,LIMIT_RL_U,LIMIT_RL_D,VR_RL);
soulx 10:53cb691e22bf 22
soulx 0:2433ddae2772 23 DigitalOut myled(LED1);
soulx 0:2433ddae2772 24 DigitalIn mybutton(USER_BUTTON);
soulx 0:2433ddae2772 25
soulx 6:8d80c84e0c09 26 //communication config
soulx 6:8d80c84e0c09 27 //serial for debug
soulx 11:336dd293daa1 28 iSerial pc(USBTX, USBRX);
soulx 6:8d80c84e0c09 29 //serial for xbee
soulx 6:8d80c84e0c09 30 COMMUNICATION pan_a(TX_XBEE, RX_XBEE,115200,1000,1000); // tx, rx
soulx 0:2433ddae2772 31
soulx 11:336dd293daa1 32 //Fuction prototye
soulx 11:336dd293daa1 33 void getCommand();
soulx 11:336dd293daa1 34 // init function
soulx 11:336dd293daa1 35 void calibration();
soulx 11:336dd293daa1 36 void test_position();
soulx 0:2433ddae2772 37
soulx 0:2433ddae2772 38
soulx 11:336dd293daa1 39 void copy_data(ANDANTE_PROTOCOL_PACKET *scr, ANDANTE_PROTOCOL_PACKET *dst);
soulx 0:2433ddae2772 40
soulx 6:8d80c84e0c09 41
soulx 6:8d80c84e0c09 42 //set foreground
soulx 6:8d80c84e0c09 43 Ticker Update_command;
soulx 6:8d80c84e0c09 44
soulx 11:336dd293daa1 45 Timer t;
soulx 11:336dd293daa1 46
soulx 11:336dd293daa1 47 //variable
soulx 11:336dd293daa1 48 volatile ANDANTE_PROTOCOL_PACKET *param;
soulx 11:336dd293daa1 49 volatile uint8_t status=0;
soulx 6:8d80c84e0c09 50
soulx 0:2433ddae2772 51 //Main function
soulx 0:2433ddae2772 52 int main()
soulx 0:2433ddae2772 53 {
soulx 11:336dd293daa1 54 int state=0;
soulx 11:336dd293daa1 55 pc.baud(115200);
soulx 11:336dd293daa1 56 pc.printf("Welcome to DOGWHEELSCHAIR\n");
soulx 11:336dd293daa1 57 if(mybutton == 0) {
soulx 11:336dd293daa1 58 calibration();
soulx 11:336dd293daa1 59 } else {
soulx 11:336dd293daa1 60 pc.printf("Lock position Min-Max...");
soulx 11:336dd293daa1 61 leg_left_upper.SetMaxPosition(56244);
soulx 11:336dd293daa1 62 leg_left_upper.SetMinPosition(20806);
soulx 11:336dd293daa1 63
soulx 11:336dd293daa1 64 leg_left_lower.SetMaxPosition(50996);
soulx 11:336dd293daa1 65 leg_left_lower.SetMinPosition(5371);
soulx 11:336dd293daa1 66
soulx 11:336dd293daa1 67 leg_right_upper.SetMaxPosition(38985);
soulx 11:336dd293daa1 68 leg_right_upper.SetMinPosition(8545);
soulx 11:336dd293daa1 69
soulx 11:336dd293daa1 70 leg_right_lower.SetMaxPosition(38027);
soulx 11:336dd293daa1 71 leg_right_lower.SetMinPosition(40);
soulx 11:336dd293daa1 72 pc.printf("pass\n");
soulx 11:336dd293daa1 73 }
soulx 11:336dd293daa1 74
soulx 11:336dd293daa1 75 Update_command.attach(&getCommand,TIMER_UPDATE);
soulx 11:336dd293daa1 76
soulx 11:336dd293daa1 77 t.start();
soulx 10:53cb691e22bf 78 while(1) {
soulx 10:53cb691e22bf 79
soulx 11:336dd293daa1 80
soulx 11:336dd293daa1 81 if(t.read() > 10.0f) {
soulx 11:336dd293daa1 82 t.reset();
soulx 11:336dd293daa1 83 if(status >3) {
soulx 11:336dd293daa1 84 status =0;
soulx 11:336dd293daa1 85 } else {
soulx 11:336dd293daa1 86 status++;
soulx 11:336dd293daa1 87 }
soulx 0:2433ddae2772 88 }
soulx 10:53cb691e22bf 89
soulx 11:336dd293daa1 90 if(status == 0) {
soulx 11:336dd293daa1 91 state =0;
soulx 11:336dd293daa1 92 // sleep
soulx 11:336dd293daa1 93 state += leg_left_upper.position_control(0);
soulx 11:336dd293daa1 94 state += leg_right_upper.position_control(0);
soulx 11:336dd293daa1 95 state += leg_left_lower.position_control(64);
soulx 11:336dd293daa1 96 state += leg_right_lower.position_control(64);
soulx 11:336dd293daa1 97 if(state == 8) {
soulx 11:336dd293daa1 98 myled=1;
soulx 11:336dd293daa1 99 } else {
soulx 11:336dd293daa1 100 myled=0;
soulx 11:336dd293daa1 101 }
soulx 11:336dd293daa1 102 } else if(status == 1 || status ==3) {
soulx 11:336dd293daa1 103 state =0;
soulx 11:336dd293daa1 104 // sit
soulx 11:336dd293daa1 105 state += leg_left_upper.position_control(64);
soulx 11:336dd293daa1 106 state += leg_right_upper.position_control(64);
soulx 11:336dd293daa1 107 state += leg_left_lower.position_control(64);
soulx 11:336dd293daa1 108 state += leg_right_lower.position_control(64);
soulx 11:336dd293daa1 109 if(state == 8) {
soulx 11:336dd293daa1 110 myled=1;
soulx 11:336dd293daa1 111 } else {
soulx 11:336dd293daa1 112 myled=0;
soulx 11:336dd293daa1 113 }
soulx 11:336dd293daa1 114 } else if(status == 2) {
soulx 11:336dd293daa1 115 state =0;
soulx 11:336dd293daa1 116 // stand
soulx 11:336dd293daa1 117 state += leg_left_upper.position_control(64);
soulx 11:336dd293daa1 118 state += leg_right_upper.position_control(64);
soulx 11:336dd293daa1 119 state += leg_left_lower.position_control(0);
soulx 11:336dd293daa1 120 state += leg_right_lower.position_control(0);
soulx 11:336dd293daa1 121 if(state == 8) {
soulx 11:336dd293daa1 122 myled=1;
soulx 11:336dd293daa1 123 } else {
soulx 11:336dd293daa1 124 myled=0;
soulx 11:336dd293daa1 125 }
soulx 10:53cb691e22bf 126 }
soulx 10:53cb691e22bf 127 }
soulx 10:53cb691e22bf 128
soulx 11:336dd293daa1 129 }
soulx 11:336dd293daa1 130
soulx 11:336dd293daa1 131 void getCommand()
soulx 11:336dd293daa1 132 {
soulx 11:336dd293daa1 133 static uint8_t count =0;
soulx 11:336dd293daa1 134
soulx 11:336dd293daa1 135 ANDANTE_PROTOCOL_PACKET packet;
soulx 11:336dd293daa1 136
soulx 11:336dd293daa1 137 uint8_t status = pan_a.receiveCommunicatePacket(&packet);
soulx 11:336dd293daa1 138 myled=0;
soulx 11:336dd293daa1 139
soulx 11:336dd293daa1 140 if(status == ANDANTE_ERRBIT_NONE) {
soulx 11:336dd293daa1 141 if(count >2 && count <10) {
soulx 11:336dd293daa1 142 count--;
soulx 11:336dd293daa1 143 } else {
soulx 11:336dd293daa1 144 count=0;
soulx 5:f07cbb5a86c3 145 }
soulx 11:336dd293daa1 146
soulx 11:336dd293daa1 147
soulx 11:336dd293daa1 148 // pan_a.sendCommunicatePacket(&packet);
soulx 11:336dd293daa1 149
soulx 11:336dd293daa1 150 //update command
soulx 11:336dd293daa1 151 //setControl(&packet);
soulx 11:336dd293daa1 152
soulx 11:336dd293daa1 153 } else if(status == ANDANTE_ERRBIT_READ_TIMEOUT) {
soulx 11:336dd293daa1 154 count++;
soulx 11:336dd293daa1 155 }
soulx 5:f07cbb5a86c3 156
soulx 11:336dd293daa1 157 if(count >= TIMEOUT_RESPONE_COMMAND) {
soulx 11:336dd293daa1 158 //stop robot
soulx 11:336dd293daa1 159 count++;
soulx 11:336dd293daa1 160 myled=1;
soulx 11:336dd293daa1 161 // setSpeedControl(0.0);
soulx 11:336dd293daa1 162 }
soulx 11:336dd293daa1 163 }
soulx 0:2433ddae2772 164
soulx 11:336dd293daa1 165 void calibration()
soulx 11:336dd293daa1 166 {
soulx 11:336dd293daa1 167 //calibration
soulx 11:336dd293daa1 168 pc.printf("Calibration [START]\n");
soulx 11:336dd293daa1 169 leg_left_upper.calibration();
soulx 11:336dd293daa1 170 pc.printf("Left_UPPER\n");
soulx 11:336dd293daa1 171 pc.printf("max_position = %d\n",leg_left_upper.GetMaxPosition());
soulx 11:336dd293daa1 172 pc.printf("min_position = %d\n",leg_left_upper.GetMinPosition());
soulx 11:336dd293daa1 173 leg_left_lower.calibration();
soulx 11:336dd293daa1 174 pc.printf("Left_Lower\n");
soulx 11:336dd293daa1 175 pc.printf("max_position = %d\n",leg_left_lower.GetMaxPosition());
soulx 11:336dd293daa1 176 pc.printf("min_position = %d\n",leg_left_lower.GetMinPosition());
soulx 11:336dd293daa1 177 leg_right_upper.calibration();
soulx 11:336dd293daa1 178 pc.printf("right_UPPER\n");
soulx 11:336dd293daa1 179 pc.printf("max_position = %d\n",leg_right_upper.GetMaxPosition());
soulx 11:336dd293daa1 180 pc.printf("min_position = %d\n",leg_right_upper.GetMinPosition());
soulx 11:336dd293daa1 181 leg_right_lower.calibration();
soulx 11:336dd293daa1 182 pc.printf("right_Lower\n");
soulx 11:336dd293daa1 183 pc.printf("max_position = %d\n",leg_right_lower.GetMaxPosition());
soulx 11:336dd293daa1 184 pc.printf("min_position = %d\n",leg_right_lower.GetMinPosition());
soulx 0:2433ddae2772 185
soulx 11:336dd293daa1 186 pc.printf("Calibration [FINISH]\n");
soulx 11:336dd293daa1 187 pc.printf("RUN mode [START]\n");
soulx 11:336dd293daa1 188 wait(1);
soulx 11:336dd293daa1 189 }
soulx 11:336dd293daa1 190
soulx 11:336dd293daa1 191 void test_position()
soulx 11:336dd293daa1 192 {
soulx 11:336dd293daa1 193 uint8_t state;
soulx 11:336dd293daa1 194 do {
soulx 10:53cb691e22bf 195 /*
soulx 10:53cb691e22bf 196 state=0;
soulx 10:53cb691e22bf 197 //leg_left_upper.position_control(500);
soulx 10:53cb691e22bf 198 if(leg_left_lower.position_control(500) ==2)
soulx 10:53cb691e22bf 199 state++;
soulx 10:53cb691e22bf 200 if(leg_right_upper.position_control(500) == 2)
soulx 10:53cb691e22bf 201 state++;
soulx 10:53cb691e22bf 202 if(leg_right_lower.position_control(500) == 2)
soulx 10:53cb691e22bf 203 state++;
soulx 10:53cb691e22bf 204 */
soulx 10:53cb691e22bf 205 state = leg_left_upper.position_control(32);
soulx 10:53cb691e22bf 206 pc.printf("state_lu %d\n",state);
soulx 10:53cb691e22bf 207 state = leg_left_lower.position_control(32);
soulx 10:53cb691e22bf 208 pc.printf("state_ll %d\n",state);
soulx 10:53cb691e22bf 209 state = leg_right_upper.position_control(32);
soulx 10:53cb691e22bf 210 pc.printf("state_ru %d\n",state);
soulx 11:336dd293daa1 211 state = leg_right_lower.position_control(32);
soulx 10:53cb691e22bf 212 pc.printf("state_rl %d\n",state);
soulx 10:53cb691e22bf 213 state=0;
soulx 11:336dd293daa1 214 } while(state != 2);
soulx 10:53cb691e22bf 215 pc.printf("[Finish test]\n");
soulx 6:8d80c84e0c09 216 }