worasuchad haomachai / LamborSteering

DC motor position control with potentiometer

Dependents:   Ackermann steering baseControl_ackermannCar

LamborSteer.cpp@0:b20808175db0, 2018-11-07 (annotated)

Committer:
worasuchad
Date:
Wed Nov 07 16:51:16 2018 +0000
Revision:
0:b20808175db0
DC motor position control with potentiometer

Who changed what in which revision?

UserRevisionLine numberNew contents of line
worasuchad 0:b20808175db0 1 #include "LamborSteer.h"
worasuchad 0:b20808175db0 2 #include "mbed.h"
worasuchad 0:b20808175db0 3
worasuchad 0:b20808175db0 4 LamborSteer::LamborSteer(PinName _cw_pin, PinName _ccw_pin, PinName _enablerPin, PinName _sensor_pin, int _offset)
worasuchad 0:b20808175db0 5 : cw_pin(_cw_pin), ccw_pin(_ccw_pin), enabler_pin(_enablerPin), sensor_pin(_sensor_pin)
worasuchad 0:b20808175db0 6 {
worasuchad 0:b20808175db0 7 if (s_index == MAX_SERVOS)
worasuchad 0:b20808175db0 8 {
worasuchad 0:b20808175db0 9 return;
worasuchad 0:b20808175db0 10 }
worasuchad 0:b20808175db0 11
worasuchad 0:b20808175db0 12 index = s_index;
worasuchad 0:b20808175db0 13
worasuchad 0:b20808175db0 14 _m_servos[s_index++] = this;
worasuchad 0:b20808175db0 15
worasuchad 0:b20808175db0 16 tolerance = 15;
worasuchad 0:b20808175db0 17
worasuchad 0:b20808175db0 18 /* New */
worasuchad 0:b20808175db0 19 offset = _offset;
worasuchad 0:b20808175db0 20 active = true;
worasuchad 0:b20808175db0 21
worasuchad 0:b20808175db0 22 cw_pin = 0;
worasuchad 0:b20808175db0 23 ccw_pin = 0;
worasuchad 0:b20808175db0 24 }
worasuchad 0:b20808175db0 25
worasuchad 0:b20808175db0 26 // timer interrupt routine
worasuchad 0:b20808175db0 27 void LamborSteer::timer_int_routine()
worasuchad 0:b20808175db0 28 {
worasuchad 0:b20808175db0 29 for (int i = 0; i < s_index; i++)
worasuchad 0:b20808175db0 30 {
worasuchad 0:b20808175db0 31 if (_m_servos[i]->active)
worasuchad 0:b20808175db0 32 {
worasuchad 0:b20808175db0 33 _m_servos[i]->update();
worasuchad 0:b20808175db0 34 }
worasuchad 0:b20808175db0 35 }
worasuchad 0:b20808175db0 36 }
worasuchad 0:b20808175db0 37
worasuchad 0:b20808175db0 38
worasuchad 0:b20808175db0 39 void LamborSteer::detach()
worasuchad 0:b20808175db0 40 {
worasuchad 0:b20808175db0 41 stop();
worasuchad 0:b20808175db0 42
worasuchad 0:b20808175db0 43 active = false;
worasuchad 0:b20808175db0 44 }
worasuchad 0:b20808175db0 45
worasuchad 0:b20808175db0 46 void LamborSteer::write(int degrees)
worasuchad 0:b20808175db0 47 {
worasuchad 0:b20808175db0 48 // map degrees to potentiometer value
worasuchad 0:b20808175db0 49 int pot_value = map(degrees, MIN_ANGLE, MAX_ANGLE, MIN_POT_VALUE, MAX_POT_VALUE);
worasuchad 0:b20808175db0 50
worasuchad 0:b20808175db0 51 write_pot_value(pot_value);
worasuchad 0:b20808175db0 52 }
worasuchad 0:b20808175db0 53
worasuchad 0:b20808175db0 54 void LamborSteer::update()
worasuchad 0:b20808175db0 55 {
worasuchad 0:b20808175db0 56 int current_position = get_position();
worasuchad 0:b20808175db0 57
worasuchad 0:b20808175db0 58 // Guard servo's min and max bounds
worasuchad 0:b20808175db0 59 if (current_position < MIN_POT_VALUE)
worasuchad 0:b20808175db0 60 {
worasuchad 0:b20808175db0 61 write(MIN_ANGLE);
worasuchad 0:b20808175db0 62 }
worasuchad 0:b20808175db0 63
worasuchad 0:b20808175db0 64 if (current_position > MAX_POT_VALUE)
worasuchad 0:b20808175db0 65 {
worasuchad 0:b20808175db0 66 write(MAX_ANGLE);
worasuchad 0:b20808175db0 67 }
worasuchad 0:b20808175db0 68
worasuchad 0:b20808175db0 69 // Gap between curent and wanted positions
worasuchad 0:b20808175db0 70 int gap = abs(current_position - wanted_position);
worasuchad 0:b20808175db0 71
worasuchad 0:b20808175db0 72 // If gap is within tolerance then stop
worasuchad 0:b20808175db0 73 if (gap < tolerance)
worasuchad 0:b20808175db0 74 {
worasuchad 0:b20808175db0 75 stop();
worasuchad 0:b20808175db0 76
worasuchad 0:b20808175db0 77 return;
worasuchad 0:b20808175db0 78 }
worasuchad 0:b20808175db0 79
worasuchad 0:b20808175db0 80 // set speed according to gap remaining
worasuchad 0:b20808175db0 81 speed = 100; //125
worasuchad 0:b20808175db0 82
worasuchad 0:b20808175db0 83 if (gap > 50)
worasuchad 0:b20808175db0 84 {
worasuchad 0:b20808175db0 85 speed = 100; //165
worasuchad 0:b20808175db0 86 }
worasuchad 0:b20808175db0 87 if (gap > 75)
worasuchad 0:b20808175db0 88 {
worasuchad 0:b20808175db0 89 speed = 100; //205
worasuchad 0:b20808175db0 90 }
worasuchad 0:b20808175db0 91 if (gap > 100)
worasuchad 0:b20808175db0 92 {
worasuchad 0:b20808175db0 93 speed = 100; //255
worasuchad 0:b20808175db0 94 }
worasuchad 0:b20808175db0 95
worasuchad 0:b20808175db0 96 // Determine direction
worasuchad 0:b20808175db0 97 if (current_position < wanted_position)
worasuchad 0:b20808175db0 98 {
worasuchad 0:b20808175db0 99 run_cw();
worasuchad 0:b20808175db0 100 }
worasuchad 0:b20808175db0 101
worasuchad 0:b20808175db0 102 if (current_position > wanted_position)
worasuchad 0:b20808175db0 103 {
worasuchad 0:b20808175db0 104 run_ccw();
worasuchad 0:b20808175db0 105 }
worasuchad 0:b20808175db0 106 }
worasuchad 0:b20808175db0 107
worasuchad 0:b20808175db0 108 servo_status LamborSteer::read()
worasuchad 0:b20808175db0 109 {
worasuchad 0:b20808175db0 110 // Get current postion, wanted position and speed
worasuchad 0:b20808175db0 111 servo_status current_status;
worasuchad 0:b20808175db0 112
worasuchad 0:b20808175db0 113 int current_angle = map(get_position(), MIN_POT_VALUE, MAX_POT_VALUE, MIN_ANGLE, MAX_ANGLE);
worasuchad 0:b20808175db0 114 int wanted_angle = map(wanted_position, MIN_POT_VALUE, MAX_POT_VALUE, MIN_ANGLE, MAX_ANGLE);
worasuchad 0:b20808175db0 115
worasuchad 0:b20808175db0 116 current_status.current_angle = current_angle;
worasuchad 0:b20808175db0 117 current_status.wanted_angle = wanted_angle;
worasuchad 0:b20808175db0 118 current_status.speed = speed;
worasuchad 0:b20808175db0 119
worasuchad 0:b20808175db0 120 return current_status;
worasuchad 0:b20808175db0 121 }
worasuchad 0:b20808175db0 122
worasuchad 0:b20808175db0 123 /*************** Private stuff **************/
worasuchad 0:b20808175db0 124
worasuchad 0:b20808175db0 125 void LamborSteer::write_pot_value(int value)
worasuchad 0:b20808175db0 126 {
worasuchad 0:b20808175db0 127 // Constrain value to servo's min and max bounds
worasuchad 0:b20808175db0 128 //int constrained_value = constrain(value, MIN_POT_VALUE + tolerance, MAX_POT_VALUE - tolerance);
worasuchad 0:b20808175db0 129 if(value > MAX_POT_VALUE - tolerance)
worasuchad 0:b20808175db0 130 {
worasuchad 0:b20808175db0 131 constrained_value = MAX_POT_VALUE - tolerance;
worasuchad 0:b20808175db0 132 }
worasuchad 0:b20808175db0 133 else if(value < MIN_POT_VALUE + tolerance)
worasuchad 0:b20808175db0 134 {
worasuchad 0:b20808175db0 135 constrained_value = MIN_POT_VALUE + tolerance;
worasuchad 0:b20808175db0 136 }
worasuchad 0:b20808175db0 137 else
worasuchad 0:b20808175db0 138 {
worasuchad 0:b20808175db0 139 constrained_value = value;
worasuchad 0:b20808175db0 140 }
worasuchad 0:b20808175db0 141 // set destination
worasuchad 0:b20808175db0 142 wanted_position = constrained_value;
worasuchad 0:b20808175db0 143 }
worasuchad 0:b20808175db0 144
worasuchad 0:b20808175db0 145 // Run motor clockwise at current speed
worasuchad 0:b20808175db0 146 void LamborSteer::run_cw()
worasuchad 0:b20808175db0 147 {
worasuchad 0:b20808175db0 148 cw_pin = 1;
worasuchad 0:b20808175db0 149 ccw_pin = 0;
worasuchad 0:b20808175db0 150
worasuchad 0:b20808175db0 151 enabler_pin.write(speed / 255.0f);
worasuchad 0:b20808175db0 152 }
worasuchad 0:b20808175db0 153
worasuchad 0:b20808175db0 154 // Run motor counter clockwise at current speed
worasuchad 0:b20808175db0 155 void LamborSteer::run_ccw()
worasuchad 0:b20808175db0 156 {
worasuchad 0:b20808175db0 157 cw_pin = 0;
worasuchad 0:b20808175db0 158 ccw_pin = 1;
worasuchad 0:b20808175db0 159
worasuchad 0:b20808175db0 160 enabler_pin.write(speed / 255.0f);
worasuchad 0:b20808175db0 161 }
worasuchad 0:b20808175db0 162
worasuchad 0:b20808175db0 163 // Stop the motor, set speed to 0
worasuchad 0:b20808175db0 164 void LamborSteer::stop()
worasuchad 0:b20808175db0 165 {
worasuchad 0:b20808175db0 166 cw_pin = 0;
worasuchad 0:b20808175db0 167 ccw_pin = 0;
worasuchad 0:b20808175db0 168
worasuchad 0:b20808175db0 169 enabler_pin.write(0);
worasuchad 0:b20808175db0 170 }
worasuchad 0:b20808175db0 171
worasuchad 0:b20808175db0 172 // Get current offset potentiometer position
worasuchad 0:b20808175db0 173 int LamborSteer::get_position()
worasuchad 0:b20808175db0 174 {
worasuchad 0:b20808175db0 175 return (sensor_pin.read()*1023) + offset; // Read the analog input value (value from 0.0 to 1.0)
worasuchad 0:b20808175db0 176 // And convert to 0 - 1023
worasuchad 0:b20808175db0 177 }
worasuchad 0:b20808175db0 178
worasuchad 0:b20808175db0 179 // Mapping
worasuchad 0:b20808175db0 180 long LamborSteer::map(long x, long in_min, long in_max, long out_min, long out_max)
worasuchad 0:b20808175db0 181 {
worasuchad 0:b20808175db0 182 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
worasuchad 0:b20808175db0 183 }