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
main.cpp@10:53cb691e22bf, 2015-07-18 (annotated)
- Committer:
- soulx
- Date:
- Sat Jul 18 05:53:10 2015 +0000
- Revision:
- 10:53cb691e22bf
- Parent:
- 9:0de6ce956985
- Child:
- 11:336dd293daa1
Who changed what in which revision?
| User | Revision | Line number | New 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 | 6:8d80c84e0c09 | 6 | |
| soulx | 10:53cb691e22bf | 7 | #include "motor_relay.h" |
| soulx | 10:53cb691e22bf | 8 | #include "motion_control.h" |
| soulx | 10:53cb691e22bf | 9 | |
| soulx | 6:8d80c84e0c09 | 10 | //set frequancy unit in Hz |
| soulx | 6:8d80c84e0c09 | 11 | #define F_UPDATE 10.0f |
| soulx | 6:8d80c84e0c09 | 12 | #define TIMER_UPDATE 1.0f/F_UPDATE |
| soulx | 6:8d80c84e0c09 | 13 | |
| soulx | 6:8d80c84e0c09 | 14 | //counter not receive from station |
| soulx | 6:8d80c84e0c09 | 15 | #define TIMEOUT_RESPONE_COMMAND 5 |
| soulx | 10:53cb691e22bf | 16 | /* |
| soulx | 0:2433ddae2772 | 17 | //define pin class |
| soulx | 0:2433ddae2772 | 18 | DigitalOut dirA_LU(INA_L_U); |
| soulx | 0:2433ddae2772 | 19 | DigitalOut dirB_LU(INB_L_U); |
| soulx | 0:2433ddae2772 | 20 | |
| soulx | 0:2433ddae2772 | 21 | DigitalOut dirA_LL(INA_L_L); |
| soulx | 0:2433ddae2772 | 22 | DigitalOut dirB_LL(INB_L_L); |
| soulx | 0:2433ddae2772 | 23 | |
| soulx | 0:2433ddae2772 | 24 | DigitalOut dirA_RU(INA_R_U); |
| soulx | 0:2433ddae2772 | 25 | DigitalOut dirB_RU(INB_R_U); |
| soulx | 0:2433ddae2772 | 26 | |
| soulx | 0:2433ddae2772 | 27 | DigitalOut dirA_RL(INA_R_L); |
| soulx | 0:2433ddae2772 | 28 | DigitalOut dirB_RL(INB_R_L); |
| soulx | 0:2433ddae2772 | 29 | |
| soulx | 0:2433ddae2772 | 30 | DigitalIn sw_LU_U(LIMIT_LU_U,PullUp); |
| soulx | 0:2433ddae2772 | 31 | DigitalIn sw_LU_D(LIMIT_LU_D,PullUp); |
| soulx | 0:2433ddae2772 | 32 | |
| soulx | 0:2433ddae2772 | 33 | DigitalIn sw_LL_U(LIMIT_LL_U,PullUp); |
| soulx | 0:2433ddae2772 | 34 | DigitalIn sw_LL_D(LIMIT_LL_D,PullUp); |
| soulx | 0:2433ddae2772 | 35 | |
| soulx | 0:2433ddae2772 | 36 | DigitalIn sw_RU_U(LIMIT_RU_U,PullUp); |
| soulx | 0:2433ddae2772 | 37 | DigitalIn sw_RU_D(LIMIT_RU_D,PullUp); |
| soulx | 0:2433ddae2772 | 38 | |
| soulx | 0:2433ddae2772 | 39 | DigitalIn sw_RL_U(LIMIT_RL_U,PullUp); |
| soulx | 0:2433ddae2772 | 40 | DigitalIn sw_RL_D(LIMIT_RL_D,PullUp); |
| soulx | 0:2433ddae2772 | 41 | |
| soulx | 0:2433ddae2772 | 42 | AnalogIn position_LU(VR_LU); |
| soulx | 0:2433ddae2772 | 43 | AnalogIn position_LL(VR_LL); |
| soulx | 0:2433ddae2772 | 44 | AnalogIn position_RU(VR_RU); |
| soulx | 0:2433ddae2772 | 45 | AnalogIn position_RL(VR_RL); |
| soulx | 10:53cb691e22bf | 46 | */ |
| soulx | 0:2433ddae2772 | 47 | |
| soulx | 10:53cb691e22bf | 48 | |
| soulx | 10:53cb691e22bf | 49 | MOTION_CONTROL leg_left_upper(INA_L_U,INB_L_U,LIMIT_LU_U,LIMIT_LU_D,VR_LU); |
| soulx | 10:53cb691e22bf | 50 | MOTION_CONTROL leg_left_lower(INA_L_L,INB_L_L,LIMIT_LL_U,LIMIT_LL_D,VR_LL); |
| soulx | 10:53cb691e22bf | 51 | MOTION_CONTROL leg_right_upper(INA_R_U,INB_R_U,LIMIT_RU_U,LIMIT_RU_D,VR_RU); |
| soulx | 10:53cb691e22bf | 52 | MOTION_CONTROL leg_right_lower(INA_R_L,INB_R_L,LIMIT_RL_U,LIMIT_RL_D,VR_RL); |
| soulx | 10:53cb691e22bf | 53 | |
| soulx | 10:53cb691e22bf | 54 | /* |
| soulx | 10:53cb691e22bf | 55 | MOTOR_RELAY leg_left_upper(INA_L_U,INB_L_U); |
| soulx | 10:53cb691e22bf | 56 | MOTION_CONTROL leg_left_lower(INA_L_L,INB_L_L,LIMIT_LL_U,LIMIT_LL_D,VR_LL); |
| soulx | 10:53cb691e22bf | 57 | MOTION_CONTROL leg_right_upper(INA_R_U,INB_R_U,LIMIT_RU_U,LIMIT_RU_D,VR_RU); |
| soulx | 10:53cb691e22bf | 58 | MOTION_CONTROL leg_right_lower(INA_R_L,INB_R_L,LIMIT_RL_U,LIMIT_RL_D,VR_RL); |
| soulx | 10:53cb691e22bf | 59 | */ |
| soulx | 0:2433ddae2772 | 60 | DigitalOut myled(LED1); |
| soulx | 0:2433ddae2772 | 61 | DigitalIn mybutton(USER_BUTTON); |
| soulx | 0:2433ddae2772 | 62 | |
| soulx | 6:8d80c84e0c09 | 63 | //communication config |
| soulx | 6:8d80c84e0c09 | 64 | //serial for debug |
| soulx | 0:2433ddae2772 | 65 | Serial pc(USBTX, USBRX); |
| soulx | 6:8d80c84e0c09 | 66 | //serial for xbee |
| soulx | 6:8d80c84e0c09 | 67 | COMMUNICATION pan_a(TX_XBEE, RX_XBEE,115200,1000,1000); // tx, rx |
| soulx | 0:2433ddae2772 | 68 | |
| soulx | 0:2433ddae2772 | 69 | //Function Prototype |
| soulx | 10:53cb691e22bf | 70 | //void motor_set(uint8_t id, uint8_t direct); |
| soulx | 10:53cb691e22bf | 71 | //void motor_stop(uint8_t id); |
| soulx | 0:2433ddae2772 | 72 | |
| soulx | 0:2433ddae2772 | 73 | uint8_t limit_motor(uint8_t id, uint8_t dirction); |
| soulx | 0:2433ddae2772 | 74 | uint8_t position_control(uint8_t id, uint16_t current, uint16_t target); |
| soulx | 0:2433ddae2772 | 75 | |
| soulx | 0:2433ddae2772 | 76 | void calibration(uint8_t id); |
| soulx | 0:2433ddae2772 | 77 | |
| soulx | 6:8d80c84e0c09 | 78 | void getCommand(); |
| soulx | 6:8d80c84e0c09 | 79 | |
| soulx | 6:8d80c84e0c09 | 80 | //set foreground |
| soulx | 6:8d80c84e0c09 | 81 | Ticker Update_command; |
| soulx | 6:8d80c84e0c09 | 82 | |
| soulx | 6:8d80c84e0c09 | 83 | |
| soulx | 0:2433ddae2772 | 84 | //Main function |
| soulx | 0:2433ddae2772 | 85 | int main() |
| soulx | 0:2433ddae2772 | 86 | { |
| soulx | 10:53cb691e22bf | 87 | |
| soulx | 10:53cb691e22bf | 88 | int state; |
| soulx | 10:53cb691e22bf | 89 | int state_count=0; |
| soulx | 10:53cb691e22bf | 90 | unsigned int count=0; |
| soulx | 10:53cb691e22bf | 91 | |
| soulx | 10:53cb691e22bf | 92 | /* |
| soulx | 10:53cb691e22bf | 93 | while(1) { |
| soulx | 10:53cb691e22bf | 94 | |
| soulx | 10:53cb691e22bf | 95 | if(count < 800000) { |
| soulx | 10:53cb691e22bf | 96 | count++; |
| soulx | 10:53cb691e22bf | 97 | } else { |
| soulx | 10:53cb691e22bf | 98 | count=0; |
| soulx | 10:53cb691e22bf | 99 | state_count= ~state_count; |
| soulx | 0:2433ddae2772 | 100 | } |
| soulx | 10:53cb691e22bf | 101 | |
| soulx | 10:53cb691e22bf | 102 | if(state_count ==0) { |
| soulx | 10:53cb691e22bf | 103 | state = leg_left_upper.limit_motor(1); |
| soulx | 10:53cb691e22bf | 104 | |
| soulx | 10:53cb691e22bf | 105 | state = leg_left_lower.limit_motor(1); |
| soulx | 10:53cb691e22bf | 106 | |
| soulx | 10:53cb691e22bf | 107 | // if(state ==1) { |
| soulx | 10:53cb691e22bf | 108 | // pc.printf("state[LU1]=1\n"); |
| soulx | 10:53cb691e22bf | 109 | // } else if(state == -1) { |
| soulx | 10:53cb691e22bf | 110 | // pc.printf("state[LU1]=-1\n"); |
| soulx | 10:53cb691e22bf | 111 | // } else { |
| soulx | 10:53cb691e22bf | 112 | // pc.printf("state[LU1]=0\n"); |
| soulx | 10:53cb691e22bf | 113 | // } |
| soulx | 10:53cb691e22bf | 114 | // pc.printf("state[LL]=%d,state\n"); |
| soulx | 10:53cb691e22bf | 115 | |
| soulx | 10:53cb691e22bf | 116 | state = leg_right_upper.limit_motor(1); |
| soulx | 10:53cb691e22bf | 117 | // pc.printf("state[RU]=%d,state\n"); |
| soulx | 10:53cb691e22bf | 118 | state = leg_right_lower.limit_motor(1); |
| soulx | 10:53cb691e22bf | 119 | // pc.printf("state[RL]=%d,state\n"); |
| soulx | 10:53cb691e22bf | 120 | |
| soulx | 10:53cb691e22bf | 121 | //wait(1); |
| soulx | 10:53cb691e22bf | 122 | } else { |
| soulx | 10:53cb691e22bf | 123 | state = leg_left_upper.limit_motor(2); |
| soulx | 10:53cb691e22bf | 124 | |
| soulx | 10:53cb691e22bf | 125 | // pc.printf("state[LU]=%d,state\n"); |
| soulx | 10:53cb691e22bf | 126 | state = leg_left_lower.limit_motor(2); |
| soulx | 10:53cb691e22bf | 127 | // if(state ==1) { |
| soulx | 10:53cb691e22bf | 128 | // pc.printf("state[LU2]=1\n"); |
| soulx | 10:53cb691e22bf | 129 | // } else if(state == -1) { |
| soulx | 10:53cb691e22bf | 130 | // pc.printf("state[LU2]=-1\n"); |
| soulx | 10:53cb691e22bf | 131 | // } else { |
| soulx | 10:53cb691e22bf | 132 | // pc.printf("state[LU2]=0\n"); |
| soulx | 10:53cb691e22bf | 133 | // } |
| soulx | 10:53cb691e22bf | 134 | // pc.printf("state[LL]=%d,state\n"); |
| soulx | 10:53cb691e22bf | 135 | |
| soulx | 10:53cb691e22bf | 136 | state = leg_right_upper.limit_motor(2); |
| soulx | 10:53cb691e22bf | 137 | // pc.printf("state[RU]=%d,state\n"); |
| soulx | 10:53cb691e22bf | 138 | state = leg_right_lower.limit_motor(2); |
| soulx | 10:53cb691e22bf | 139 | // pc.printf("state[RL]=%d,state\n\n"); |
| soulx | 10:53cb691e22bf | 140 | //wait(1); |
| soulx | 10:53cb691e22bf | 141 | } |
| soulx | 10:53cb691e22bf | 142 | } |
| soulx | 10:53cb691e22bf | 143 | |
| soulx | 0:2433ddae2772 | 144 | */ |
| soulx | 0:2433ddae2772 | 145 | /* |
| soulx | 0:2433ddae2772 | 146 | while(1) { |
| soulx | 0:2433ddae2772 | 147 | //Read position |
| soulx | 10:53cb691e22bf | 148 | pc.printf("vr_LL = %d\t",leg_left_upper.GetAnalog()); |
| soulx | 10:53cb691e22bf | 149 | pc.printf("vr_LU = %d\t",leg_left_lower.GetAnalog()); |
| soulx | 10:53cb691e22bf | 150 | pc.printf("vr_RL = %d\t",leg_right_upper.GetAnalog()); |
| soulx | 10:53cb691e22bf | 151 | pc.printf("vr_RU = %d\n",leg_right_lower.GetAnalog()); |
| soulx | 5:f07cbb5a86c3 | 152 | } |
| soulx | 5:f07cbb5a86c3 | 153 | */ |
| soulx | 5:f07cbb5a86c3 | 154 | |
| soulx | 10:53cb691e22bf | 155 | // while(1) { |
| soulx | 10:53cb691e22bf | 156 | //calibration |
| soulx | 10:53cb691e22bf | 157 | pc.printf("Welcome to DOGWHEELSCHAIR\n"); |
| soulx | 10:53cb691e22bf | 158 | pc.printf("Calibration [START]\n"); |
| soulx | 10:53cb691e22bf | 159 | leg_left_upper.calibration(); |
| soulx | 10:53cb691e22bf | 160 | pc.printf("Left_UPPER\n"); |
| soulx | 10:53cb691e22bf | 161 | pc.printf("max_position = %d\n",leg_left_upper.GetMaxPosition()); |
| soulx | 10:53cb691e22bf | 162 | pc.printf("min_position = %d\n",leg_left_upper.GetMinPosition()); |
| soulx | 10:53cb691e22bf | 163 | leg_left_lower.calibration(); |
| soulx | 10:53cb691e22bf | 164 | pc.printf("Left_Lower\n"); |
| soulx | 10:53cb691e22bf | 165 | pc.printf("max_position = %d\n",leg_left_lower.GetMaxPosition()); |
| soulx | 10:53cb691e22bf | 166 | pc.printf("min_position = %d\n",leg_left_lower.GetMinPosition()); |
| soulx | 10:53cb691e22bf | 167 | leg_right_upper.calibration(); |
| soulx | 10:53cb691e22bf | 168 | pc.printf("right_UPPER\n"); |
| soulx | 10:53cb691e22bf | 169 | pc.printf("max_position = %d\n",leg_right_upper.GetMaxPosition()); |
| soulx | 10:53cb691e22bf | 170 | pc.printf("min_position = %d\n",leg_right_upper.GetMinPosition()); |
| soulx | 10:53cb691e22bf | 171 | leg_right_lower.calibration(); |
| soulx | 10:53cb691e22bf | 172 | pc.printf("right_Lower\n"); |
| soulx | 10:53cb691e22bf | 173 | pc.printf("max_position = %d\n",leg_right_lower.GetMaxPosition()); |
| soulx | 10:53cb691e22bf | 174 | pc.printf("min_position = %d\n",leg_right_lower.GetMinPosition()); |
| soulx | 0:2433ddae2772 | 175 | |
| soulx | 10:53cb691e22bf | 176 | pc.printf("Calibration [FINISH]\n"); |
| soulx | 10:53cb691e22bf | 177 | pc.printf("RUN mode [START]\n"); |
| soulx | 10:53cb691e22bf | 178 | wait(1); |
| soulx | 10:53cb691e22bf | 179 | // } |
| soulx | 0:2433ddae2772 | 180 | |
| soulx | 6:8d80c84e0c09 | 181 | Update_command.attach(&getCommand,TIMER_UPDATE); |
| soulx | 10:53cb691e22bf | 182 | |
| soulx | 10:53cb691e22bf | 183 | do |
| soulx | 10:53cb691e22bf | 184 | { |
| soulx | 10:53cb691e22bf | 185 | /* |
| soulx | 10:53cb691e22bf | 186 | state=0; |
| soulx | 10:53cb691e22bf | 187 | //leg_left_upper.position_control(500); |
| soulx | 10:53cb691e22bf | 188 | if(leg_left_lower.position_control(500) ==2) |
| soulx | 10:53cb691e22bf | 189 | state++; |
| soulx | 10:53cb691e22bf | 190 | if(leg_right_upper.position_control(500) == 2) |
| soulx | 10:53cb691e22bf | 191 | state++; |
| soulx | 10:53cb691e22bf | 192 | if(leg_right_lower.position_control(500) == 2) |
| soulx | 10:53cb691e22bf | 193 | state++; |
| soulx | 10:53cb691e22bf | 194 | */ |
| soulx | 10:53cb691e22bf | 195 | state = leg_left_upper.position_control(32); |
| soulx | 10:53cb691e22bf | 196 | pc.printf("state_lu %d\n",state); |
| soulx | 10:53cb691e22bf | 197 | state = leg_left_lower.position_control(32); |
| soulx | 10:53cb691e22bf | 198 | pc.printf("state_ll %d\n",state); |
| soulx | 10:53cb691e22bf | 199 | state = leg_right_upper.position_control(32); |
| soulx | 10:53cb691e22bf | 200 | pc.printf("state_ru %d\n",state); |
| soulx | 10:53cb691e22bf | 201 | state = leg_right_lower.position_control(32); |
| soulx | 10:53cb691e22bf | 202 | pc.printf("state_rl %d\n",state); |
| soulx | 10:53cb691e22bf | 203 | state=0; |
| soulx | 10:53cb691e22bf | 204 | }while(state != 2); |
| soulx | 10:53cb691e22bf | 205 | pc.printf("[Finish test]\n"); |
| soulx | 0:2433ddae2772 | 206 | } |
| soulx | 9:0de6ce956985 | 207 | |
| soulx | 6:8d80c84e0c09 | 208 | void getCommand() |
| soulx | 6:8d80c84e0c09 | 209 | { |
| soulx | 6:8d80c84e0c09 | 210 | static uint8_t count =0; |
| soulx | 6:8d80c84e0c09 | 211 | |
| soulx | 6:8d80c84e0c09 | 212 | ANDANTE_PROTOCOL_PACKET packet; |
| soulx | 6:8d80c84e0c09 | 213 | |
| soulx | 6:8d80c84e0c09 | 214 | uint8_t status = pan_a.receiveCommunicatePacket(&packet); |
| soulx | 6:8d80c84e0c09 | 215 | myled=0; |
| soulx | 10:53cb691e22bf | 216 | |
| soulx | 6:8d80c84e0c09 | 217 | |
| soulx | 6:8d80c84e0c09 | 218 | |
| soulx | 6:8d80c84e0c09 | 219 | if(status == ANDANTE_ERRBIT_NONE) { |
| soulx | 6:8d80c84e0c09 | 220 | if(count >2 && count <10) { |
| soulx | 6:8d80c84e0c09 | 221 | count--; |
| soulx | 6:8d80c84e0c09 | 222 | } else { |
| soulx | 6:8d80c84e0c09 | 223 | count=0; |
| soulx | 10:53cb691e22bf | 224 | |
| soulx | 6:8d80c84e0c09 | 225 | } |
| soulx | 6:8d80c84e0c09 | 226 | pan_a.sendCommunicatePacket(&packet); |
| soulx | 6:8d80c84e0c09 | 227 | //update command |
| soulx | 6:8d80c84e0c09 | 228 | //setControl(&packet); |
| soulx | 6:8d80c84e0c09 | 229 | |
| soulx | 6:8d80c84e0c09 | 230 | } else if(status == ANDANTE_ERRBIT_READ_TIMEOUT) { |
| soulx | 6:8d80c84e0c09 | 231 | count++; |
| soulx | 6:8d80c84e0c09 | 232 | } |
| soulx | 6:8d80c84e0c09 | 233 | |
| soulx | 6:8d80c84e0c09 | 234 | if(count >= TIMEOUT_RESPONE_COMMAND) { |
| soulx | 6:8d80c84e0c09 | 235 | //stop robot |
| soulx | 6:8d80c84e0c09 | 236 | count++; |
| soulx | 6:8d80c84e0c09 | 237 | myled=1; |
| soulx | 10:53cb691e22bf | 238 | // setSpeedControl(0.0); |
| soulx | 6:8d80c84e0c09 | 239 | } |
| soulx | 6:8d80c84e0c09 | 240 | } |