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@0:77ab14788110, 2015-07-17 (annotated)

Committer:
soulx
Date:
Fri Jul 17 12:07:01 2015 +0000
Revision:
0:77ab14788110
Child:
1:5b313fd2ca6f
create motion control. No test

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 0:77ab14788110 36
soulx 0:77ab14788110 37 if(error > MARGIN) {
soulx 0:77ab14788110 38 if(limit_motor(1)==0 ) { //limit sens
soulx 0:77ab14788110 39 //pc.printf("motor[%d]=limit error\n",id);
soulx 0:77ab14788110 40 return limit_motor(1);
soulx 0:77ab14788110 41 }
soulx 0:77ab14788110 42 } else if(error < -MARGIN) {
soulx 0:77ab14788110 43 if(limit_motor(2)!=0 ) { //limit sens
soulx 0:77ab14788110 44 //pc.printf("motor[%d]=limit error\n",id);
soulx 0:77ab14788110 45 return limit_motor(2);
soulx 0:77ab14788110 46 }
soulx 0:77ab14788110 47 } else { //in zone
soulx 0:77ab14788110 48 motor->StopMotor();
soulx 0:77ab14788110 49 //pc.printf("motor[%d]=complete\n",id);
soulx 0:77ab14788110 50 return 1; //in zone complete
soulx 0:77ab14788110 51 }
soulx 0:77ab14788110 52 //pc.printf("motor[%d]=in process\n",id);
soulx 0:77ab14788110 53 return 0; //in process
soulx 0:77ab14788110 54 }
soulx 0:77ab14788110 55
soulx 0:77ab14788110 56 void MOTION_CONTROL::calibration()
soulx 0:77ab14788110 57 {
soulx 0:77ab14788110 58 //pc.printf("motor[1] run up\n");
soulx 0:77ab14788110 59 do {
soulx 0:77ab14788110 60 if(_limit_up == 0) {
soulx 0:77ab14788110 61 motor->StopMotor();
soulx 0:77ab14788110 62 } else {
soulx 0:77ab14788110 63 motor->SetMotor(1);
soulx 0:77ab14788110 64 }
soulx 0:77ab14788110 65 } while(_limit_up ==0);
soulx 0:77ab14788110 66
soulx 0:77ab14788110 67 //pc.printf("motor[1] stop up\n");
soulx 0:77ab14788110 68 wait_ms(500);
soulx 0:77ab14788110 69 do {
soulx 0:77ab14788110 70 motor->SetMotor(2);
soulx 0:77ab14788110 71 } while(_limit_up);
soulx 0:77ab14788110 72 motor->StopMotor();
soulx 0:77ab14788110 73 wait_ms(500);
soulx 0:77ab14788110 74 //pc.printf("motor[1] read position\n");
soulx 0:77ab14788110 75
soulx 0:77ab14788110 76 MAX_POSITION = _position.read_u16();
soulx 0:77ab14788110 77
soulx 0:77ab14788110 78 //pc.printf("max_pos_LU= %d\n",max_pos_LU);
soulx 0:77ab14788110 79 //pc.printf("motor[1] run down\n");
soulx 0:77ab14788110 80
soulx 0:77ab14788110 81 do {
soulx 0:77ab14788110 82 if(_limit_down == 0) {
soulx 0:77ab14788110 83 motor->StopMotor();
soulx 0:77ab14788110 84 } else {
soulx 0:77ab14788110 85 motor->SetMotor(2);
soulx 0:77ab14788110 86 }
soulx 0:77ab14788110 87 } while(_limit_down==0) ;
soulx 0:77ab14788110 88 //pc.printf("motor[1] stop down\n");
soulx 0:77ab14788110 89 do {
soulx 0:77ab14788110 90 motor->SetMotor(1);
soulx 0:77ab14788110 91 } while(_limit_down);
soulx 0:77ab14788110 92 motor->StopMotor();
soulx 0:77ab14788110 93 wait_ms(500);
soulx 0:77ab14788110 94 //pc.printf("motor[1] read position\n");
soulx 0:77ab14788110 95 MIN_POSITION = _position.read_u16();
soulx 0:77ab14788110 96 //pc.printf("min_pos_LU= %d\n",min_pos_LU);
soulx 0:77ab14788110 97
soulx 0:77ab14788110 98 }