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@4:6509fec6a6fb, 2015-07-18 (annotated)

Committer:
soulx
Date:
Sat Jul 18 05:52:56 2015 +0000
Revision:
4:6509fec6a6fb
Parent:
3:4fa191f2194d
Child:
5:91d905f8bef7
test and edit :pass

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
soulx 4:6509fec6a6fb 6 MOTION_CONTROL::MOTION_CONTROL(PinName dirA, PinName dirB, PinName limitUp, PinName limitDown, PinName vr): motor(dirA,dirB), _limit_up(limitUp), _limit_down(limitDown), _position(vr)
soulx 0:77ab14788110 7 {
soulx 4:6509fec6a6fb 8 //MOTOR_RELAY *motor = new MOTOR_RELAY(dirA,dirB);
soulx 0:77ab14788110 9 error=0;
soulx 4:6509fec6a6fb 10 MARGIN=1;
soulx 4:6509fec6a6fb 11 //scale =1000;
soulx 0:77ab14788110 12 }
soulx 0:77ab14788110 13
soulx 0:77ab14788110 14
soulx 4:6509fec6a6fb 15 int MOTION_CONTROL::limit_motor(uint8_t dirction)
soulx 0:77ab14788110 16 {
soulx 4:6509fec6a6fb 17
soulx 0:77ab14788110 18 if(_limit_up) {
soulx 4:6509fec6a6fb 19 //while(_limit_up!=0) {
soulx 4:6509fec6a6fb 20 motor.SetMotor(2);
soulx 4:6509fec6a6fb 21 //}
soulx 0:77ab14788110 22 return 1;
soulx 4:6509fec6a6fb 23 } else if(_limit_down) {
soulx 4:6509fec6a6fb 24
soulx 4:6509fec6a6fb 25 motor.SetMotor(1);
soulx 4:6509fec6a6fb 26
soulx 4:6509fec6a6fb 27 // }
soulx 4:6509fec6a6fb 28 //motor.StopMotor();
soulx 4:6509fec6a6fb 29 return -1;
soulx 0:77ab14788110 30 } else {
soulx 4:6509fec6a6fb 31 motor.SetMotor(dirction);
soulx 4:6509fec6a6fb 32 return 0;//Normally
soulx 4:6509fec6a6fb 33
soulx 0:77ab14788110 34 }
soulx 4:6509fec6a6fb 35
soulx 4:6509fec6a6fb 36 //motor.SetMotor(dirction);
soulx 4:6509fec6a6fb 37 //return 0;
soulx 0:77ab14788110 38 }
soulx 0:77ab14788110 39
soulx 4:6509fec6a6fb 40 int8_t MOTION_CONTROL::position_control(uint16_t target)
soulx 0:77ab14788110 41 {
soulx 4:6509fec6a6fb 42 uint16_t current =Scale(_position.read_u16());
soulx 4:6509fec6a6fb 43 //target = Scale(target);
soulx 4:6509fec6a6fb 44 if(target > scale || target < 0)
soulx 4:6509fec6a6fb 45 {
soulx 4:6509fec6a6fb 46 return -3;
soulx 4:6509fec6a6fb 47 }
soulx 4:6509fec6a6fb 48
soulx 0:77ab14788110 49 error = target-current;
soulx 1:5b313fd2ca6f 50 if(error >scale || error < -scale) {
soulx 4:6509fec6a6fb 51 // pc.printf("data error\n");
soulx 4:6509fec6a6fb 52 return -2;
soulx 1:5b313fd2ca6f 53 } else {
soulx 1:5b313fd2ca6f 54 if(error > MARGIN) {
soulx 1:5b313fd2ca6f 55 if(limit_motor(1)==0 ) { //limit sens
soulx 1:5b313fd2ca6f 56 //pc.printf("motor[%d]=limit error\n",id);
soulx 1:5b313fd2ca6f 57 return limit_motor(1);
soulx 1:5b313fd2ca6f 58 }
soulx 1:5b313fd2ca6f 59 } else if(error < -MARGIN) {
soulx 1:5b313fd2ca6f 60 if(limit_motor(2)!=0 ) { //limit sens
soulx 1:5b313fd2ca6f 61 //pc.printf("motor[%d]=limit error\n",id);
soulx 1:5b313fd2ca6f 62 return limit_motor(2);
soulx 1:5b313fd2ca6f 63 }
soulx 1:5b313fd2ca6f 64 } else { //in zone
soulx 4:6509fec6a6fb 65 motor.StopMotor();
soulx 1:5b313fd2ca6f 66 //pc.printf("motor[%d]=complete\n",id);
soulx 1:5b313fd2ca6f 67 return 2; //in zone complete
soulx 0:77ab14788110 68 }
soulx 0:77ab14788110 69 }
soulx 4:6509fec6a6fb 70 //pc.printf("motor[%d]=in process\n",id);
soulx 4:6509fec6a6fb 71 return 0; //in process
soulx 0:77ab14788110 72 }
soulx 0:77ab14788110 73
soulx 0:77ab14788110 74 void MOTION_CONTROL::calibration()
soulx 0:77ab14788110 75 {
soulx 4:6509fec6a6fb 76 /*
soulx 0:77ab14788110 77 //pc.printf("motor[1] run up\n");
soulx 0:77ab14788110 78 do {
soulx 4:6509fec6a6fb 79 if(_limit_up) {
soulx 4:6509fec6a6fb 80 motor.StopMotor();
soulx 0:77ab14788110 81 } else {
soulx 4:6509fec6a6fb 82 motor.SetMotor(1);
soulx 0:77ab14788110 83 }
soulx 4:6509fec6a6fb 84 } while(_limit_up);
soulx 4:6509fec6a6fb 85 motor.StopMotor();
soulx 0:77ab14788110 86 //pc.printf("motor[1] stop up\n");
soulx 0:77ab14788110 87 wait_ms(500);
soulx 0:77ab14788110 88 do {
soulx 4:6509fec6a6fb 89 motor.SetMotor(2);
soulx 0:77ab14788110 90 } while(_limit_up);
soulx 4:6509fec6a6fb 91 motor.StopMotor();
soulx 0:77ab14788110 92 wait_ms(500);
soulx 0:77ab14788110 93 //pc.printf("motor[1] read position\n");
soulx 0:77ab14788110 94
soulx 0:77ab14788110 95 MAX_POSITION = _position.read_u16();
soulx 0:77ab14788110 96
soulx 0:77ab14788110 97 //pc.printf("max_pos_LU= %d\n",max_pos_LU);
soulx 0:77ab14788110 98 //pc.printf("motor[1] run down\n");
soulx 0:77ab14788110 99
soulx 0:77ab14788110 100 do {
soulx 0:77ab14788110 101 if(_limit_down == 0) {
soulx 4:6509fec6a6fb 102 motor.StopMotor();
soulx 0:77ab14788110 103 } else {
soulx 4:6509fec6a6fb 104 motor.SetMotor(2);
soulx 0:77ab14788110 105 }
soulx 0:77ab14788110 106 } while(_limit_down==0) ;
soulx 0:77ab14788110 107 //pc.printf("motor[1] stop down\n");
soulx 0:77ab14788110 108 do {
soulx 4:6509fec6a6fb 109 motor.SetMotor(1);
soulx 0:77ab14788110 110 } while(_limit_down);
soulx 4:6509fec6a6fb 111 motor.StopMotor();
soulx 0:77ab14788110 112 wait_ms(500);
soulx 0:77ab14788110 113 //pc.printf("motor[1] read position\n");
soulx 0:77ab14788110 114 MIN_POSITION = _position.read_u16();
soulx 0:77ab14788110 115 //pc.printf("min_pos_LU= %d\n",min_pos_LU);
soulx 4:6509fec6a6fb 116 */
soulx 4:6509fec6a6fb 117 int state=0;
soulx 0:77ab14788110 118
soulx 4:6509fec6a6fb 119 do {
soulx 4:6509fec6a6fb 120 state = limit_motor(1);
soulx 4:6509fec6a6fb 121 } while(state==0);
soulx 4:6509fec6a6fb 122 //motor.StopMotor();
soulx 4:6509fec6a6fb 123 do {
soulx 4:6509fec6a6fb 124 state = limit_motor(2);
soulx 4:6509fec6a6fb 125 } while(state!=1);
soulx 4:6509fec6a6fb 126 wait_ms(200);
soulx 4:6509fec6a6fb 127 motor.StopMotor();
soulx 4:6509fec6a6fb 128 wait_ms(500);
soulx 4:6509fec6a6fb 129 MAX_POSITION = _position.read_u16();
soulx 4:6509fec6a6fb 130
soulx 4:6509fec6a6fb 131 do {
soulx 4:6509fec6a6fb 132 state = limit_motor(2);
soulx 4:6509fec6a6fb 133 } while(state==0);
soulx 4:6509fec6a6fb 134 //motor.StopMotor();
soulx 4:6509fec6a6fb 135 do {
soulx 4:6509fec6a6fb 136 state = limit_motor(1);
soulx 4:6509fec6a6fb 137 } while(state!=0);
soulx 4:6509fec6a6fb 138 wait_ms(100);
soulx 4:6509fec6a6fb 139 motor.StopMotor();
soulx 4:6509fec6a6fb 140 MIN_POSITION = _position.read_u16();
soulx 3:4fa191f2194d 141 }
soulx 3:4fa191f2194d 142
soulx 3:4fa191f2194d 143 int MOTION_CONTROL::GetLimitUp()
soulx 3:4fa191f2194d 144 {
soulx 3:4fa191f2194d 145 return _limit_up;
soulx 3:4fa191f2194d 146 }
soulx 3:4fa191f2194d 147
soulx 3:4fa191f2194d 148 int MOTION_CONTROL::GetLimitDown()
soulx 3:4fa191f2194d 149 {
soulx 3:4fa191f2194d 150 return _limit_down;
soulx 3:4fa191f2194d 151 }
soulx 3:4fa191f2194d 152
soulx 3:4fa191f2194d 153 void MOTION_CONTROL::SetMargin(int16_t data)
soulx 3:4fa191f2194d 154 {
soulx 3:4fa191f2194d 155 MARGIN = data;
soulx 3:4fa191f2194d 156 }
soulx 3:4fa191f2194d 157 int16_t MOTION_CONTROL::GetMargin()
soulx 3:4fa191f2194d 158 {
soulx 3:4fa191f2194d 159 return MARGIN;
soulx 3:4fa191f2194d 160 }
soulx 3:4fa191f2194d 161
soulx 3:4fa191f2194d 162 uint16_t MOTION_CONTROL::GetMaxPosition()
soulx 3:4fa191f2194d 163 {
soulx 3:4fa191f2194d 164 return MAX_POSITION;
soulx 3:4fa191f2194d 165 }
soulx 3:4fa191f2194d 166
soulx 3:4fa191f2194d 167 uint16_t MOTION_CONTROL::GetMinPosition()
soulx 3:4fa191f2194d 168 {
soulx 3:4fa191f2194d 169 return MIN_POSITION;
soulx 3:4fa191f2194d 170 }
soulx 4:6509fec6a6fb 171
soulx 4:6509fec6a6fb 172 uint16_t MOTION_CONTROL::Scale(uint16_t data)
soulx 4:6509fec6a6fb 173 {
soulx 4:6509fec6a6fb 174 return ((float)(data-MIN_POSITION)/(MAX_POSITION - MIN_POSITION))*scale;
soulx 4:6509fec6a6fb 175 }
soulx 4:6509fec6a6fb 176
soulx 4:6509fec6a6fb 177 uint16_t MOTION_CONTROL::GetAnalog()
soulx 4:6509fec6a6fb 178 {
soulx 4:6509fec6a6fb 179 return _position.read_u16();
soulx 4:6509fec6a6fb 180 }
soulx 4:6509fec6a6fb 181
soulx 4:6509fec6a6fb 182 uint16_t MOTION_CONTROL::GetPosition()
soulx 4:6509fec6a6fb 183 {
soulx 4:6509fec6a6fb 184 return Scale(_position.read_u16());
soulx 4:6509fec6a6fb 185 }
soulx 4:6509fec6a6fb 186
soulx 4:6509fec6a6fb 187 void MOTION_CONTROL::stop()
soulx 4:6509fec6a6fb 188 {
soulx 4:6509fec6a6fb 189 motor.StopMotor();
soulx 4:6509fec6a6fb 190 }