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
- Committer:
- soulx
- Date:
- 2015-07-15
- Revision:
- 0:2433ddae2772
- Child:
- 1:768d359e9d96
File content as of revision 0:2433ddae2772:
#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;
}
}