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@9:e3895130e56f, 2015-12-07 (annotated)

Committer:
soulx
Date:
Mon Dec 07 09:15:37 2015 +0000
Revision:
9:e3895130e56f
Parent:
7:94518aee126b 
??????????????????????

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