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:
- 0:2433ddae2772
- Child:
- 1:768d359e9d96
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Jul 15 05:07:26 2015 +0000
@@ -0,0 +1,553 @@
+#include "mbed.h"
+#include "pin_config.h"
+
+//define pin class
+DigitalOut dirA_LU(INA_L_U);
+DigitalOut dirB_LU(INB_L_U);
+
+DigitalOut dirA_LL(INA_L_L);
+DigitalOut dirB_LL(INB_L_L);
+
+DigitalOut dirA_RU(INA_R_U);
+DigitalOut dirB_RU(INB_R_U);
+
+DigitalOut dirA_RL(INA_R_L);
+DigitalOut dirB_RL(INB_R_L);
+
+DigitalIn sw_LU_U(LIMIT_LU_U,PullUp);
+DigitalIn sw_LU_D(LIMIT_LU_D,PullUp);
+
+DigitalIn sw_LL_U(LIMIT_LL_U,PullUp);
+DigitalIn sw_LL_D(LIMIT_LL_D,PullUp);
+
+DigitalIn sw_RU_U(LIMIT_RU_U,PullUp);
+DigitalIn sw_RU_D(LIMIT_RU_D,PullUp);
+
+DigitalIn sw_RL_U(LIMIT_RL_U,PullUp);
+DigitalIn sw_RL_D(LIMIT_RL_D,PullUp);
+
+AnalogIn position_LU(VR_LU);
+AnalogIn position_LL(VR_LL);
+AnalogIn position_RU(VR_RU);
+AnalogIn position_RL(VR_RL);
+
+DigitalOut myled(LED1);
+DigitalIn mybutton(USER_BUTTON);
+
+Serial pc(USBTX, USBRX);
+
+
+//Function Prototype
+void motor_set(uint8_t id, uint8_t direct);
+void motor_stop(uint8_t id);
+
+uint8_t limit_motor(uint8_t id, uint8_t dirction);
+uint8_t position_control(uint8_t id, uint16_t current, uint16_t target);
+
+void calibration(uint8_t id);
+
+
+//Globle Variable
+uint16_t max_pos_LU= 10000;
+uint16_t min_pos_LU= 6000;
+uint16_t max_pos_LL= 50000;
+uint16_t min_pos_LL= 37000;
+
+uint16_t max_pos_RU= 17800;
+uint16_t min_pos_RU= 9000;
+uint16_t max_pos_RL= 51000;
+uint16_t min_pos_RL= 11000;
+
+int16_t MARGIN = 500;
+
+//Main function
+int main()
+{
+ uint16_t vr_lu,vr_ru;
+ uint16_t vr_ll,vr_rl;
+ pc.printf("wait\n");
+ motor_stop(0);
+ // wait(10);
+ /*
+ while(1) {
+ motor_set(1,1);
+ motor_set(2,1);
+ motor_set(3,1);
+ motor_set(4,1);
+ wait(1);
+ motor_set(1,2);
+ motor_set(2,2);
+ motor_set(3,2);
+ motor_set(4,2);
+ wait(1);
+ }
+ */
+
+ /*
+ while(1) {
+ //Read position
+ vr_ll = position_LL.read_u16();
+ vr_lu = position_LU.read_u16();
+ vr_rl = position_RL.read_u16();
+ vr_ru = position_RU.read_u16();
+ pc.printf("vr_LL = %d\t",vr_ll);
+ pc.printf("vr_LU = %d\t",vr_lu);
+ pc.printf("vr_RL = %d\t",vr_rl);
+ pc.printf("vr_RU = %d\n",vr_ru);
+ }
+ */
+
+ /*
+ while(1) {
+ myled =1;
+ wait_ms(200);
+
+ if(mybutton == 0) {
+ myled =0;
+ wait_ms(200);
+ }
+
+ if(sw_LU_U == 0) {
+ myled =0;
+ wait_ms(200);
+ }
+
+ if(sw_LU_D == 0) {
+ myled =0;
+ wait_ms(200);
+ }
+
+ if(sw_LL_U == 0) {
+ myled =0;
+ wait_ms(200);
+ }
+
+ if(sw_LL_D == 0) {
+ myled =0;
+ wait_ms(200);
+ }
+
+ if(sw_RU_U == 0) {
+ myled =0;
+ wait_ms(200);
+ }
+
+ if(sw_RU_D == 0) {
+ myled =0;
+ wait_ms(200);
+ }
+
+ if(sw_RL_U == 0) {
+ myled =0;
+ wait_ms(200);
+ }
+
+ if(sw_RL_D == 0) {
+ myled =0;
+ wait_ms(200);
+ }
+
+ }
+ */
+
+ // while(1) {
+ //calibration
+ pc.printf("Welcome to DOGWHEELSCHAIR\n");
+ pc.printf("Calibration [START]\n");
+ calibration(1);
+ calibration(2);
+ calibration(3);
+ calibration(4);
+ pc.printf("Calibration [FINISH]\n");
+ pc.printf("RUN mode [START]\n");
+ // }
+
+
+ uint8_t count=0;
+
+ while(1) {
+ uint8_t state=0;
+ pc.printf("Count %d",count);
+ do {
+ state=0;
+
+ if(position_control(1,position_LU.read_u16(),max_pos_LU) == 1) {
+ state++;
+ }
+
+ if(position_control(2,position_LL.read_u16(),max_pos_LL) == 1) {
+ state++;
+ }
+
+ if(position_control(3,position_RU.read_u16(),max_pos_RU) == 1) {
+ state++;
+ }
+
+ if(position_control(4,position_RL.read_u16(),max_pos_RL) == 1) {
+ state++;
+ }
+
+ pc.printf("state = %d",state);
+ } while(state <= 4 );
+
+ do {
+ state=0;
+
+ if(position_control(1,position_LU.read_u16(),min_pos_LU) == 1) {
+ state++;
+ }
+
+ if(position_control(2,position_LL.read_u16(),min_pos_LL) == 1) {
+ state++;
+ }
+
+ if(position_control(3,position_RU.read_u16(),min_pos_RU) == 1) {
+ state++;
+ }
+
+ if(position_control(4,position_RL.read_u16(),min_pos_RL) == 1) {
+ state++;
+ }
+
+ pc.printf("state = %d",state);
+ } while(state <= 4 );
+ count++;
+ }
+}
+
+
+
+void motor_set(uint8_t id, uint8_t direct)
+{
+ //direct: Should be between 0 and 3, with the following result
+ //0: Brake to VCC
+ //1: Clockwise
+ //2: CounterClockwise
+ //3: Brake to GND
+
+ if(direct <=4) {
+ switch(id) {
+ case 1:
+ // Set inA[motor]
+ if (direct <=1)
+ dirA_LU=0;
+ else
+ dirA_LU=1;
+
+ // Set inB[motor]
+ if ((direct==0)||(direct==2))
+ dirB_LU=0;
+ else
+ dirB_LU=1;
+ break;
+
+ case 2:
+ // Set inA[motor]
+ if (direct <=1)
+ dirA_LL=0;
+ else
+ dirA_LL=1;
+
+ // Set inB[motor]
+ if ((direct==0)||(direct==2))
+ dirB_LL=0;
+ else
+ dirB_LL=1;
+ break;
+
+ case 3:
+ // Set inA[motor]
+ if (direct <=1)
+ dirA_RU=0;
+ else
+ dirA_RU=1;
+
+ // Set inB[motor]
+ if ((direct==0)||(direct==2))
+ dirB_RU=0;
+ else
+ dirB_RU=1;
+ break;
+
+ case 4:
+ // Set inA[motor]
+ if (direct <=1)
+ dirA_RL=0;
+ else
+ dirA_RL=1;
+ // Set inB[motor]
+ if ((direct==0)||(direct==2))
+ dirB_RL=0;
+ else
+ dirB_RL=1;
+ break;
+ }
+ }
+}
+
+void motor_stop(uint8_t id)
+{
+ switch(id) {
+ case 1:
+ dirA_LU=1;
+ dirB_LU=1;
+ break;
+
+ case 2:
+ dirA_LL=1;
+ dirB_LL=1;
+ break;
+
+ case 3:
+ dirA_RU=1;
+ dirB_RU=1;
+ break;
+
+ case 4:
+ dirA_RL=1;
+ dirB_RL=1;
+ break;
+
+ case 0:
+ dirA_LU=1;
+ dirB_LU=1;
+
+ dirA_LL=1;
+ dirB_LL=1;
+
+ dirA_RU=1;
+ dirB_RU=1;
+
+ dirA_RL=1;
+ dirB_RL=1;
+ break;
+ }
+}
+
+
+
+uint8_t limit_motor(uint8_t id, uint8_t dirction)
+{
+ switch(id) {
+ case 1://Left Upper
+ if(sw_LU_U && sw_LU_D) {
+ motor_set(id,dirction);
+ } else {
+ motor_stop(id);
+ return 0;
+ }
+ break;
+
+ case 2://Left Lowwer
+ if(sw_LL_U && sw_LL_D) {
+ motor_set(id,dirction);
+ } else {
+ motor_stop(id);
+ return 0;
+ }
+ break;
+
+ case 3://Right Upper
+ if(sw_RU_U && sw_RU_D) {
+ motor_set(id,dirction);
+ } else {
+ motor_stop(id);
+ return 0;
+ }
+ break;
+
+ case 4://Right Lowwer
+ if(sw_RL_U && sw_RL_D) {
+ motor_set(id,dirction);
+ } else {
+ motor_stop(id);
+ return 0;
+ }
+ break;
+ }
+ return 1;//normally
+}
+
+
+uint8_t position_control(uint8_t id, uint16_t current, uint16_t target)
+{
+ //uint8_t state=0;
+
+
+ int16_t error = target-current;
+
+ pc.printf("error[%d]=%d\n",id,error);
+ if(error > MARGIN) {
+ if(limit_motor(id,1)==0 ) { //limit sens
+
+ pc.printf("motor[%d]=limit error\n",id);
+
+ return 2;
+ }
+
+
+ } else if(error < -MARGIN) {
+ if(limit_motor(id,2)==0 ) { //limit sens
+
+ pc.printf("motor[%d]=limit error\n",id);
+
+ return 2;
+ }
+
+ } else { //in zone
+ motor_stop(2);
+
+ pc.printf("motor[%d]=complete\n",id);
+
+ return 1; //in zone complete
+ }
+
+ pc.printf("motor[%d]=in process\n",id);
+ return 0; //in process
+}
+
+void calibration(uint8_t id)
+{
+ switch(id) {
+ case 1:
+ pc.printf("motor[1] run up\n");
+ while(sw_LU_U) {
+ motor_set(id,1);
+ }
+ pc.printf("motor[1] stop up\n");
+ //wait_ms(500);
+ do {
+ motor_set(id,2);
+ } while(sw_LU_U ==0);
+ motor_stop(id);
+ wait_ms(500);
+ pc.printf("motor[1] read position\n");
+ max_pos_LU = position_LU.read_u16();
+ pc.printf("max_pos_LU= %d\n",max_pos_LU);
+
+ pc.printf("motor[1] run down\n");
+ while(sw_LU_D) {
+ motor_set(id,2);
+ }
+ pc.printf("motor[1] stop down\n");
+ do {
+ motor_set(id,1);
+ } while(sw_LU_D ==0);
+ motor_stop(id);
+ wait_ms(500);
+ pc.printf("motor[1] read position\n");
+ min_pos_LU = position_LU.read_u16();
+ pc.printf("min_pos_LU= %d\n",min_pos_LU);
+ break;
+
+ case 2:
+
+ pc.printf("motor[2] run up\n");
+ while(sw_LL_U) {
+ motor_set(id,1);
+ }
+ motor_stop(id);
+ wait_ms(500);
+ pc.printf("motor[2] stop up\n");
+ do {
+ motor_set(id,2);
+ } while(sw_LL_U == 0);
+ motor_stop(id);
+ wait_ms(500);
+ max_pos_LL = position_LL.read_u16();
+ pc.printf("max_pos_LL= %d\n",max_pos_LL);
+ pc.printf("motor[2] run down\n");
+ while(sw_LL_D) {
+ motor_set(id,2);
+ }
+ motor_stop(id);
+ wait_ms(500);
+ pc.printf("motor[2] stop down\n");
+ do {
+ motor_set(id,1);
+ } while(sw_LL_D == 0);
+ motor_stop(id);
+ wait_ms(500);
+ min_pos_LL = position_LL.read_u16();
+ pc.printf("min_pos_LL= %d\n",min_pos_LL);
+ break;
+
+ case 3:
+ uint8_t count=1000;
+ pc.printf("motor[3] run up\n");
+ while(sw_RU_U) {
+ motor_set(id,1);
+ }
+ motor_stop(id);
+ wait_ms(500);
+ pc.printf("motor[3] stop up\n");
+
+ do {
+ motor_set(id,2);
+
+ } while(sw_RU_U ==0);
+ motor_stop(id);
+ wait_ms(500);
+ max_pos_RU = position_RU.read_u16();
+ pc.printf("max_pos_RU= %d\n",max_pos_RU);
+
+ pc.printf("motor[3] run down\n");
+
+ /*while(count >1) {
+ motor_set(id,2);
+ if(sw_RU_D)
+ {
+ count--;
+ }
+ }*/
+ while(sw_RU_D)
+ {
+
+ motor_set(id,2);
+ }
+ motor_stop(id);
+ wait_ms(500);
+ pc.printf("motor[3] stop down\n");
+ do {
+ motor_set(id,1);
+ } while(sw_RU_D == 0);
+ motor_stop(id);
+ wait_ms(500);
+ min_pos_RU = position_RU.read_u16();
+ pc.printf("min_pos_RU= %d\n",min_pos_RU);
+ break;
+
+ case 4:
+
+ pc.printf("motor[4] run up\n");
+ while(sw_RL_U) {
+ motor_set(id,1);
+ }
+ motor_stop(id);
+ wait_ms(500);
+ pc.printf("motor[4] stop up\n");
+ do {
+ motor_set(id,2);
+ } while(sw_RL_U==0);
+ motor_stop(id);
+ wait_ms(500);
+ max_pos_RL = position_RL.read_u16();
+ pc.printf("max_pos_RL= %d\n",max_pos_RL);
+ pc.printf("motor[4] run down\n");
+ while(sw_RL_D) {
+ motor_set(id,2);
+ }
+ motor_stop(id);
+ wait_ms(500);
+ pc.printf("motor[4] stop down\n");
+ do {
+ motor_set(id,1);
+ } while(sw_RL_D == 0);
+ motor_stop(id);
+ wait_ms(500);
+ min_pos_RL = position_RL.read_u16();
+ pc.printf("min_pos_RL= %d\n",min_pos_RL);
+ break;
+ }
+
+}
\ No newline at end of file