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@5:91d905f8bef7, 2015-07-19 (annotated)

Committer:
soulx
Date:
Sun Jul 19 17:38:21 2015 +0000
Revision:
5:91d905f8bef7
Parent:
4:6509fec6a6fb
Child:
6:41d7cf11fdb1
add function getpoten

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 5:91d905f8bef7 44 if(target > scale || target < 0) {
soulx 4:6509fec6a6fb 45 return -3;
soulx 4:6509fec6a6fb 46 }
soulx 5:91d905f8bef7 47
soulx 0:77ab14788110 48 error = target-current;
soulx 1:5b313fd2ca6f 49 if(error >scale || error < -scale) {
soulx 4:6509fec6a6fb 50 // pc.printf("data error\n");
soulx 4:6509fec6a6fb 51 return -2;
soulx 1:5b313fd2ca6f 52 } else {
soulx 1:5b313fd2ca6f 53 if(error > MARGIN) {
soulx 1:5b313fd2ca6f 54 if(limit_motor(1)==0 ) { //limit sens
soulx 1:5b313fd2ca6f 55 //pc.printf("motor[%d]=limit error\n",id);
soulx 1:5b313fd2ca6f 56 return limit_motor(1);
soulx 1:5b313fd2ca6f 57 }
soulx 1:5b313fd2ca6f 58 } else if(error < -MARGIN) {
soulx 1:5b313fd2ca6f 59 if(limit_motor(2)!=0 ) { //limit sens
soulx 1:5b313fd2ca6f 60 //pc.printf("motor[%d]=limit error\n",id);
soulx 1:5b313fd2ca6f 61 return limit_motor(2);
soulx 1:5b313fd2ca6f 62 }
soulx 1:5b313fd2ca6f 63 } else { //in zone
soulx 4:6509fec6a6fb 64 motor.StopMotor();
soulx 1:5b313fd2ca6f 65 //pc.printf("motor[%d]=complete\n",id);
soulx 1:5b313fd2ca6f 66 return 2; //in zone complete
soulx 0:77ab14788110 67 }
soulx 0:77ab14788110 68 }
soulx 4:6509fec6a6fb 69 //pc.printf("motor[%d]=in process\n",id);
soulx 4:6509fec6a6fb 70 return 0; //in process
soulx 0:77ab14788110 71 }
soulx 0:77ab14788110 72
soulx 0:77ab14788110 73 void MOTION_CONTROL::calibration()
soulx 0:77ab14788110 74 {
soulx 4:6509fec6a6fb 75 int state=0;
soulx 5:91d905f8bef7 76 uint32_t sum=0;
soulx 0:77ab14788110 77
soulx 4:6509fec6a6fb 78 do {
soulx 4:6509fec6a6fb 79 state = limit_motor(1);
soulx 4:6509fec6a6fb 80 } while(state==0);
soulx 5:91d905f8bef7 81
soulx 4:6509fec6a6fb 82 do {
soulx 4:6509fec6a6fb 83 state = limit_motor(2);
soulx 4:6509fec6a6fb 84 } while(state!=1);
soulx 4:6509fec6a6fb 85 wait_ms(200);
soulx 4:6509fec6a6fb 86 motor.StopMotor();
soulx 4:6509fec6a6fb 87 wait_ms(500);
soulx 5:91d905f8bef7 88
soulx 5:91d905f8bef7 89 sum=0;
soulx 5:91d905f8bef7 90 for(int a=0; a<LOOP; a++) {
soulx 5:91d905f8bef7 91 sum += _position.read_u16();
soulx 5:91d905f8bef7 92 }
soulx 5:91d905f8bef7 93 MAX_POSITION = sum/LOOP;
soulx 4:6509fec6a6fb 94
soulx 4:6509fec6a6fb 95 do {
soulx 4:6509fec6a6fb 96 state = limit_motor(2);
soulx 4:6509fec6a6fb 97 } while(state==0);
soulx 4:6509fec6a6fb 98 //motor.StopMotor();
soulx 4:6509fec6a6fb 99 do {
soulx 4:6509fec6a6fb 100 state = limit_motor(1);
soulx 4:6509fec6a6fb 101 } while(state!=0);
soulx 4:6509fec6a6fb 102 wait_ms(100);
soulx 4:6509fec6a6fb 103 motor.StopMotor();
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 MIN_POSITION = sum/LOOP;
soulx 3:4fa191f2194d 110 }
soulx 3:4fa191f2194d 111
soulx 3:4fa191f2194d 112 int MOTION_CONTROL::GetLimitUp()
soulx 3:4fa191f2194d 113 {
soulx 3:4fa191f2194d 114 return _limit_up;
soulx 3:4fa191f2194d 115 }
soulx 3:4fa191f2194d 116
soulx 3:4fa191f2194d 117 int MOTION_CONTROL::GetLimitDown()
soulx 3:4fa191f2194d 118 {
soulx 3:4fa191f2194d 119 return _limit_down;
soulx 3:4fa191f2194d 120 }
soulx 3:4fa191f2194d 121
soulx 3:4fa191f2194d 122 void MOTION_CONTROL::SetMargin(int16_t data)
soulx 3:4fa191f2194d 123 {
soulx 3:4fa191f2194d 124 MARGIN = data;
soulx 3:4fa191f2194d 125 }
soulx 3:4fa191f2194d 126 int16_t MOTION_CONTROL::GetMargin()
soulx 3:4fa191f2194d 127 {
soulx 3:4fa191f2194d 128 return MARGIN;
soulx 3:4fa191f2194d 129 }
soulx 3:4fa191f2194d 130
soulx 3:4fa191f2194d 131 uint16_t MOTION_CONTROL::GetMaxPosition()
soulx 3:4fa191f2194d 132 {
soulx 3:4fa191f2194d 133 return MAX_POSITION;
soulx 3:4fa191f2194d 134 }
soulx 3:4fa191f2194d 135
soulx 3:4fa191f2194d 136 uint16_t MOTION_CONTROL::GetMinPosition()
soulx 3:4fa191f2194d 137 {
soulx 3:4fa191f2194d 138 return MIN_POSITION;
soulx 3:4fa191f2194d 139 }
soulx 4:6509fec6a6fb 140
soulx 5:91d905f8bef7 141 void MOTION_CONTROL::SetMaxPosition(uint16_t value)
soulx 5:91d905f8bef7 142 {
soulx 5:91d905f8bef7 143 MAX_POSITION = value;
soulx 5:91d905f8bef7 144 }
soulx 5:91d905f8bef7 145 void MOTION_CONTROL::SetMinPosition(uint16_t value)
soulx 5:91d905f8bef7 146 {
soulx 5:91d905f8bef7 147 MIN_POSITION = value;
soulx 5:91d905f8bef7 148 }
soulx 5:91d905f8bef7 149
soulx 4:6509fec6a6fb 150 uint16_t MOTION_CONTROL::Scale(uint16_t data)
soulx 4:6509fec6a6fb 151 {
soulx 4:6509fec6a6fb 152 return ((float)(data-MIN_POSITION)/(MAX_POSITION - MIN_POSITION))*scale;
soulx 4:6509fec6a6fb 153 }
soulx 4:6509fec6a6fb 154
soulx 4:6509fec6a6fb 155 uint16_t MOTION_CONTROL::GetAnalog()
soulx 4:6509fec6a6fb 156 {
soulx 4:6509fec6a6fb 157 return _position.read_u16();
soulx 4:6509fec6a6fb 158 }
soulx 4:6509fec6a6fb 159
soulx 4:6509fec6a6fb 160 uint16_t MOTION_CONTROL::GetPosition()
soulx 4:6509fec6a6fb 161 {
soulx 4:6509fec6a6fb 162 return Scale(_position.read_u16());
soulx 4:6509fec6a6fb 163 }
soulx 4:6509fec6a6fb 164
soulx 4:6509fec6a6fb 165 void MOTION_CONTROL::stop()
soulx 4:6509fec6a6fb 166 {
soulx 4:6509fec6a6fb 167 motor.StopMotor();
soulx 4:6509fec6a6fb 168 }