BE@R lab / motion_control

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:   motor_relay

Dependents:   dog_V3_3_testmotor

motion_control.cpp@1:5b313fd2ca6f, 2015-07-17 (annotated)

Committer:
soulx
Date:
Fri Jul 17 14:04:47 2015 +0000
Revision:
1:5b313fd2ca6f
Parent:
0:77ab14788110
Child:
3:4fa191f2194d
reversion from test; pin_config

Who changed what in which revision?

UserRevisionLine numberNew contents of line
soulx 0:77ab14788110 1 #include "mbed.h"
soulx 0:77ab14788110 2 #include "motion_control.h"
soulx 0:77ab14788110 3 #include "motor_relay.h"
soulx 0:77ab14788110 4
soulx 0:77ab14788110 5 //int16_t MOTION_CONTROL::error=0;
soulx 0:77ab14788110 6 //int16_t MOTION_CONTROL::MARGIN=500;
soulx 0:77ab14788110 7
soulx 0:77ab14788110 8
soulx 0:77ab14788110 9 MOTION_CONTROL::MOTION_CONTROL(PinName dirA, PinName dirB, PinName limitUp, PinName limitDown, PinName vr): _limit_up(limitUp), _limit_down(limitDown), _position(vr)
soulx 0:77ab14788110 10 {
soulx 0:77ab14788110 11 MOTOR_RELAY *motor = new MOTOR_RELAY(dirA,dirB);
soulx 0:77ab14788110 12 error=0;
soulx 0:77ab14788110 13 MARGIN=500;
soulx 0:77ab14788110 14 }
soulx 0:77ab14788110 15
soulx 0:77ab14788110 16
soulx 0:77ab14788110 17 int8_t MOTION_CONTROL::limit_motor(uint8_t dirction)
soulx 0:77ab14788110 18 {
soulx 0:77ab14788110 19 if(_limit_up) {
soulx 0:77ab14788110 20 motor->StopMotor();
soulx 0:77ab14788110 21 return 1;
soulx 0:77ab14788110 22 } else {
soulx 0:77ab14788110 23 if(_limit_down) {
soulx 0:77ab14788110 24 motor->StopMotor();
soulx 0:77ab14788110 25 return -1;
soulx 0:77ab14788110 26 } else {
soulx 0:77ab14788110 27 motor->SetMotor(dirction);
soulx 0:77ab14788110 28 return 0;//Normally
soulx 0:77ab14788110 29 }
soulx 0:77ab14788110 30 }
soulx 0:77ab14788110 31 }
soulx 0:77ab14788110 32
soulx 0:77ab14788110 33 int8_t MOTION_CONTROL::position_control(uint16_t current, uint16_t target)
soulx 0:77ab14788110 34 {
soulx 0:77ab14788110 35 error = target-current;
soulx 1:5b313fd2ca6f 36 if(error >scale || error < -scale) {
soulx 1:5b313fd2ca6f 37 //pc.printf("data error\n");
soulx 1:5b313fd2ca6f 38
soulx 1:5b313fd2ca6f 39 } else {
soulx 1:5b313fd2ca6f 40 if(error > MARGIN) {
soulx 1:5b313fd2ca6f 41 if(limit_motor(1)==0 ) { //limit sens
soulx 1:5b313fd2ca6f 42 //pc.printf("motor[%d]=limit error\n",id);
soulx 1:5b313fd2ca6f 43 return limit_motor(1);
soulx 1:5b313fd2ca6f 44 }
soulx 1:5b313fd2ca6f 45 } else if(error < -MARGIN) {
soulx 1:5b313fd2ca6f 46 if(limit_motor(2)!=0 ) { //limit sens
soulx 1:5b313fd2ca6f 47 //pc.printf("motor[%d]=limit error\n",id);
soulx 1:5b313fd2ca6f 48 return limit_motor(2);
soulx 1:5b313fd2ca6f 49 }
soulx 1:5b313fd2ca6f 50 } else { //in zone
soulx 1:5b313fd2ca6f 51 motor->StopMotor();
soulx 1:5b313fd2ca6f 52 //pc.printf("motor[%d]=complete\n",id);
soulx 1:5b313fd2ca6f 53 return 2; //in zone complete
soulx 0:77ab14788110 54 }
soulx 1:5b313fd2ca6f 55
soulx 1:5b313fd2ca6f 56 //pc.printf("motor[%d]=in process\n",id);
soulx 1:5b313fd2ca6f 57 return 0; //in process
soulx 0:77ab14788110 58 }
soulx 0:77ab14788110 59 }
soulx 0:77ab14788110 60
soulx 0:77ab14788110 61 void MOTION_CONTROL::calibration()
soulx 0:77ab14788110 62 {
soulx 0:77ab14788110 63 //pc.printf("motor[1] run up\n");
soulx 0:77ab14788110 64 do {
soulx 0:77ab14788110 65 if(_limit_up == 0) {
soulx 0:77ab14788110 66 motor->StopMotor();
soulx 0:77ab14788110 67 } else {
soulx 0:77ab14788110 68 motor->SetMotor(1);
soulx 0:77ab14788110 69 }
soulx 0:77ab14788110 70 } while(_limit_up ==0);
soulx 0:77ab14788110 71
soulx 0:77ab14788110 72 //pc.printf("motor[1] stop up\n");
soulx 0:77ab14788110 73 wait_ms(500);
soulx 0:77ab14788110 74 do {
soulx 0:77ab14788110 75 motor->SetMotor(2);
soulx 0:77ab14788110 76 } while(_limit_up);
soulx 0:77ab14788110 77 motor->StopMotor();
soulx 0:77ab14788110 78 wait_ms(500);
soulx 0:77ab14788110 79 //pc.printf("motor[1] read position\n");
soulx 0:77ab14788110 80
soulx 0:77ab14788110 81 MAX_POSITION = _position.read_u16();
soulx 0:77ab14788110 82
soulx 0:77ab14788110 83 //pc.printf("max_pos_LU= %d\n",max_pos_LU);
soulx 0:77ab14788110 84 //pc.printf("motor[1] run down\n");
soulx 0:77ab14788110 85
soulx 0:77ab14788110 86 do {
soulx 0:77ab14788110 87 if(_limit_down == 0) {
soulx 0:77ab14788110 88 motor->StopMotor();
soulx 0:77ab14788110 89 } else {
soulx 0:77ab14788110 90 motor->SetMotor(2);
soulx 0:77ab14788110 91 }
soulx 0:77ab14788110 92 } while(_limit_down==0) ;
soulx 0:77ab14788110 93 //pc.printf("motor[1] stop down\n");
soulx 0:77ab14788110 94 do {
soulx 0:77ab14788110 95 motor->SetMotor(1);
soulx 0:77ab14788110 96 } while(_limit_down);
soulx 0:77ab14788110 97 motor->StopMotor();
soulx 0:77ab14788110 98 wait_ms(500);
soulx 0:77ab14788110 99 //pc.printf("motor[1] read position\n");
soulx 0:77ab14788110 100 MIN_POSITION = _position.read_u16();
soulx 0:77ab14788110 101 //pc.printf("min_pos_LU= %d\n",min_pos_LU);
soulx 0:77ab14788110 102
soulx 0:77ab14788110 103 }