E=MC / Mbed 2 deprecated e=mc

.

Dependencies:   Servo mbed

main.cpp@11:d07a4a683289, 2015-03-06 (annotated)

Committer:
mawk2311
Date:
Fri Mar 06 09:16:02 2015 +0000
Revision:
11:d07a4a683289
Parent:
10:de7a56fb94bc
Child:
12:46d0ff953a3f
PWMout for both motors

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ericoneill 0:d328ecb3fbb1 1 #include "mbed.h"
ericoneill 0:d328ecb3fbb1 2
ericoneill 0:d328ecb3fbb1 3 DigitalOut myled(LED1);
ericoneill 0:d328ecb3fbb1 4 PwmOut servo(PTA5);
mawk2311 11:d07a4a683289 5 PwmOut motor1(PTA4);
mawk2311 11:d07a4a683289 6 PwmOut motor2(PTA12);
ericoneill 0:d328ecb3fbb1 7 Serial pc(USBTX, USBRX); // tx, rx
ericoneill 3:7eaf505f811e 8
ericoneill 3:7eaf505f811e 9 // encoder setup and variables
ericoneill 3:7eaf505f811e 10 InterruptIn interrupt(PTA13);
ericoneill 4:263bddc51c0f 11
mawk2311 7:ea395616348c 12 //Intervals used during encoder data collection to measure velocity
ericoneill 4:263bddc51c0f 13 int interval1=0;
ericoneill 4:263bddc51c0f 14 int interval2=0;
ericoneill 4:263bddc51c0f 15 int interval3=0;
ericoneill 4:263bddc51c0f 16 int avg_interval=0;
ericoneill 4:263bddc51c0f 17 int lastchange1 = 0;
ericoneill 4:263bddc51c0f 18 int lastchange2 = 0;
ericoneill 4:263bddc51c0f 19 int lastchange3 = 0;
ericoneill 4:263bddc51c0f 20 int lastchange4 = 0;
mawk2311 5:61a0a21134f7 21
mawk2311 7:ea395616348c 22 //Variables used to for velocity control
mawk2311 7:ea395616348c 23 float avg_speed = 0;
mawk2311 7:ea395616348c 24 float stall_check = 0;
mawk2311 7:ea395616348c 25 float tuning_val = 1;
mawk2311 5:61a0a21134f7 26
ericoneill 1:8e5821dec0f7 27 Timer t;
ericoneill 8:f3e0b4814888 28 Timer servoTimer;
ericoneill 8:f3e0b4814888 29
ericoneill 8:f3e0b4814888 30 // Servo parameters
ericoneill 8:f3e0b4814888 31 float lastTurnTime = 0.0f;
ericoneill 8:f3e0b4814888 32 bool servoLeft = true;
mawk2311 7:ea395616348c 33
mawk2311 7:ea395616348c 34 //Observed average speeds for each duty cycle
mawk2311 7:ea395616348c 35 const float TUNING_CONSTANT_20 = 3.00;
mawk2311 7:ea395616348c 36 const float TUNING_CONSTANT_30 = 4.30;
mawk2311 5:61a0a21134f7 37 const float TUNING_CONSTANT_50 = 6.880;
ericoneill 4:263bddc51c0f 38 const float PI = 3.14159;
ericoneill 4:263bddc51c0f 39 const float WHEEL_CIRCUMFERENCE = .05*PI;
ericoneill 0:d328ecb3fbb1 40
mawk2311 7:ea395616348c 41 //Velocity Control Tuning Constants
mawk2311 7:ea395616348c 42 const float TUNE_THRESH = 0.5f;
mawk2311 7:ea395616348c 43 const float TUNE_AMT = 0.1f;
mawk2311 7:ea395616348c 44
mawk2311 7:ea395616348c 45 //Parameters specifying sample sizes and delays for small and large average speed samples
mawk2311 7:ea395616348c 46 float num_samples_small = 10.0f;
mawk2311 7:ea395616348c 47 float delay_small = 0.05f;
mawk2311 7:ea395616348c 48 float num_samples_large = 100.0f;
mawk2311 7:ea395616348c 49 float delay_large = 0.1f;
mawk2311 7:ea395616348c 50
mawk2311 7:ea395616348c 51 // Large and small arrays used to get average velocity values
mawk2311 7:ea395616348c 52 float large_avg_speed_list [100];
mawk2311 7:ea395616348c 53 float small_avg_speed_list [10];
mawk2311 7:ea395616348c 54
mawk2311 7:ea395616348c 55 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~Functions~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
ericoneill 4:263bddc51c0f 56 float get_speed(){
ericoneill 4:263bddc51c0f 57 float revPerSec = (1.0f/((float)avg_interval*.000001))*.25f;
ericoneill 4:263bddc51c0f 58 float linearSpeed = revPerSec * WHEEL_CIRCUMFERENCE;
ericoneill 4:263bddc51c0f 59 return linearSpeed;
ericoneill 4:263bddc51c0f 60 }
mawk2311 5:61a0a21134f7 61
mawk2311 7:ea395616348c 62 float get_avg_speed(float num_samples, float delay) {
mawk2311 5:61a0a21134f7 63
mawk2311 5:61a0a21134f7 64 float avg_avg_speed = 0;
mawk2311 5:61a0a21134f7 65
mawk2311 7:ea395616348c 66 for (int c = 0; c < num_samples; c++) {
mawk2311 7:ea395616348c 67 if (num_samples == num_samples_small){
mawk2311 7:ea395616348c 68 small_avg_speed_list[c] = get_speed();
mawk2311 7:ea395616348c 69 } else if (num_samples == num_samples_large){
mawk2311 7:ea395616348c 70 large_avg_speed_list[c] = get_speed();
mawk2311 7:ea395616348c 71 //pc.printf("\n\rworking: %f", large_avg_speed_list[c]);
mawk2311 7:ea395616348c 72 }
mawk2311 7:ea395616348c 73 wait(delay);
mawk2311 5:61a0a21134f7 74 }
mawk2311 5:61a0a21134f7 75
mawk2311 7:ea395616348c 76 for (int c = 1; c < num_samples; c++) {
mawk2311 7:ea395616348c 77 if (num_samples == num_samples_small){
mawk2311 7:ea395616348c 78 avg_avg_speed += small_avg_speed_list[c];
mawk2311 7:ea395616348c 79 } else if (num_samples == num_samples_large){
mawk2311 7:ea395616348c 80 avg_avg_speed += large_avg_speed_list[c];
mawk2311 7:ea395616348c 81 //pc.printf("\n\rworking: %f", large_avg_speed_list[c]);
mawk2311 7:ea395616348c 82 }
mawk2311 5:61a0a21134f7 83 }
mawk2311 7:ea395616348c 84 return avg_avg_speed/num_samples;
mawk2311 5:61a0a21134f7 85 }
mawk2311 5:61a0a21134f7 86
mawk2311 7:ea395616348c 87 void velocity_control(float duty_cyc, float tuning_const) {
mawk2311 7:ea395616348c 88
mawk2311 7:ea395616348c 89 avg_speed = get_avg_speed(num_samples_small, delay_small);
mawk2311 7:ea395616348c 90
mawk2311 7:ea395616348c 91 if (avg_speed == stall_check) {
mawk2311 7:ea395616348c 92 avg_speed = 0;
mawk2311 7:ea395616348c 93 tuning_val += TUNE_AMT;
mawk2311 7:ea395616348c 94 } else if((avg_speed - tuning_const) > TUNE_THRESH){
mawk2311 7:ea395616348c 95 tuning_val -= TUNE_AMT;
mawk2311 7:ea395616348c 96 stall_check = avg_speed;
mawk2311 7:ea395616348c 97 } else if (avg_speed - tuning_const < -1*TUNE_THRESH){
mawk2311 7:ea395616348c 98 tuning_val += TUNE_AMT;
mawk2311 7:ea395616348c 99 stall_check = avg_speed;
mawk2311 7:ea395616348c 100 } else {
mawk2311 7:ea395616348c 101 tuning_val = 1;
mawk2311 7:ea395616348c 102 stall_check = avg_speed;
mawk2311 7:ea395616348c 103 }
mawk2311 11:d07a4a683289 104 motor1.pulsewidth(.0025 * duty_cyc * tuning_val);
mawk2311 11:d07a4a683289 105 motor2.pulsewidth(.0025 * duty_cyc * tuning_val);
mawk2311 7:ea395616348c 106
mawk2311 7:ea395616348c 107 pc.printf("speed: %f\n\rtuning val: %f\n\r", avg_speed, tuning_val);
mawk2311 7:ea395616348c 108 wait(.2);
mawk2311 7:ea395616348c 109 }
mawk2311 7:ea395616348c 110
ericoneill 0:d328ecb3fbb1 111 void servo_sweep(){
ericoneill 0:d328ecb3fbb1 112 for(float p = 0.001; p<0.002; p+=0.0001){
ericoneill 0:d328ecb3fbb1 113 servo.pulsewidth(p);
ericoneill 0:d328ecb3fbb1 114 wait(0.5);
ericoneill 0:d328ecb3fbb1 115 }
ericoneill 0:d328ecb3fbb1 116 }
mawk2311 7:ea395616348c 117
ericoneill 3:7eaf505f811e 118 void fallInterrupt(){
ericoneill 4:263bddc51c0f 119
ericoneill 4:263bddc51c0f 120 int time = t.read_us();
ericoneill 4:263bddc51c0f 121 interval1 = time - lastchange2;
ericoneill 4:263bddc51c0f 122 interval2 = lastchange1-lastchange3;
ericoneill 4:263bddc51c0f 123 interval3 = lastchange2 - lastchange4;
ericoneill 4:263bddc51c0f 124 avg_interval = (interval1 + interval2 + interval3)/3;
ericoneill 4:263bddc51c0f 125
ericoneill 4:263bddc51c0f 126 lastchange4 = lastchange3;
ericoneill 4:263bddc51c0f 127 lastchange3 = lastchange2;
ericoneill 4:263bddc51c0f 128 lastchange2 = lastchange1;
ericoneill 4:263bddc51c0f 129 lastchange1 = time;
ericoneill 4:263bddc51c0f 130 //pc.printf("dark to light time : %d\n\r", interval);
ericoneill 4:263bddc51c0f 131 //pc.printf("fall");
ericoneill 3:7eaf505f811e 132 }
ericoneill 3:7eaf505f811e 133 void riseInterrupt(){
ericoneill 4:263bddc51c0f 134 int time = t.read_us();
ericoneill 4:263bddc51c0f 135 interval1 = time - lastchange2;
ericoneill 4:263bddc51c0f 136 interval2 = lastchange1-lastchange3;
ericoneill 4:263bddc51c0f 137 interval3 = lastchange2 - lastchange4;
ericoneill 4:263bddc51c0f 138 avg_interval = (interval1 + interval2 + interval3)/3;
ericoneill 4:263bddc51c0f 139
ericoneill 4:263bddc51c0f 140 lastchange4 = lastchange3;
ericoneill 4:263bddc51c0f 141 lastchange3 = lastchange2;
ericoneill 4:263bddc51c0f 142 lastchange2 = lastchange1;
ericoneill 4:263bddc51c0f 143 lastchange1 = time;
ericoneill 4:263bddc51c0f 144 //pc.printf("light to dark time : %d\n\r", interval);
ericoneill 4:263bddc51c0f 145 //pc.printf("rise");
ericoneill 3:7eaf505f811e 146 }
ericoneill 3:7eaf505f811e 147
ericoneill 0:d328ecb3fbb1 148 int main() {
ericoneill 0:d328ecb3fbb1 149 servo.period(0.005);
mawk2311 11:d07a4a683289 150 motor1.period(.0025);
mawk2311 11:d07a4a683289 151 motor2.period(.0025);
ericoneill 3:7eaf505f811e 152 interrupt.fall(&fallInterrupt);
ericoneill 3:7eaf505f811e 153 interrupt.rise(&riseInterrupt);
ericoneill 3:7eaf505f811e 154
ericoneill 1:8e5821dec0f7 155 t.start();
ericoneill 4:263bddc51c0f 156 while(1){
ericoneill 2:30ebae0d3e17 157
mawk2311 11:d07a4a683289 158 char choice = '2';//pc.getc();
mawk2311 5:61a0a21134f7 159 pc.putc(choice);
mawk2311 5:61a0a21134f7 160
ericoneill 2:30ebae0d3e17 161 switch(choice){
ericoneill 0:d328ecb3fbb1 162 case '0':
mawk2311 11:d07a4a683289 163 motor1.pulsewidth(0.0);
mawk2311 11:d07a4a683289 164 motor2.pulsewidth(0.0);
ericoneill 2:30ebae0d3e17 165 pc.printf("0% \n\r");
ericoneill 4:263bddc51c0f 166
ericoneill 0:d328ecb3fbb1 167 break;
ericoneill 0:d328ecb3fbb1 168 case '1':
mawk2311 11:d07a4a683289 169 motor1.pulsewidth(.0025);
mawk2311 11:d07a4a683289 170 motor2.pulsewidth(.0025);
ericoneill 2:30ebae0d3e17 171 pc.printf("100% \n\r");
ericoneill 4:263bddc51c0f 172 wait(.5);
mawk2311 7:ea395616348c 173 pc.printf("speed: %f",get_avg_speed(num_samples_small, delay_small));
mawk2311 5:61a0a21134f7 174
ericoneill 4:263bddc51c0f 175 break;
ericoneill 4:263bddc51c0f 176 case '2':
mawk2311 11:d07a4a683289 177 motor1.pulsewidth(.0025*.2);
mawk2311 11:d07a4a683289 178 motor2.pulsewidth(.0025*.2);
mawk2311 7:ea395616348c 179 pc.printf("\n\r20% \n\r");
mawk2311 5:61a0a21134f7 180 wait(.5);
mawk2311 7:ea395616348c 181 pc.printf("speed: %f\n\rtuning val: %f\n\r", get_avg_speed(num_samples_small, delay_small));
mawk2311 10:de7a56fb94bc 182 servoTimer.start();
mawk2311 5:61a0a21134f7 183 while(1){
mawk2311 10:de7a56fb94bc 184
mawk2311 7:ea395616348c 185 velocity_control(0.2f, TUNING_CONSTANT_20);
ericoneill 8:f3e0b4814888 186 if(servoLeft){
ericoneill 8:f3e0b4814888 187 servo.pulsewidth(.001);
ericoneill 8:f3e0b4814888 188 }
ericoneill 8:f3e0b4814888 189 else{
ericoneill 8:f3e0b4814888 190 servo.pulsewidth(.002);
ericoneill 8:f3e0b4814888 191 }
ericoneill 8:f3e0b4814888 192 float turnTime = servoTimer.read();
ericoneill 8:f3e0b4814888 193 if(turnTime - lastTurnTime > 3.0){
ericoneill 8:f3e0b4814888 194 servoLeft = !servoLeft;
ericoneill 8:f3e0b4814888 195 lastTurnTime = turnTime;
ericoneill 8:f3e0b4814888 196 }
mawk2311 5:61a0a21134f7 197 }
mawk2311 5:61a0a21134f7 198
mawk2311 7:ea395616348c 199 //break;
ericoneill 0:d328ecb3fbb1 200 case '3':
mawk2311 11:d07a4a683289 201 motor1.pulsewidth(.0025*.3);
mawk2311 11:d07a4a683289 202 motor2.pulsewidth(.0025*.3);
mawk2311 7:ea395616348c 203 pc.printf("\n\r30% \n\r");
ericoneill 4:263bddc51c0f 204 wait(.5);
mawk2311 7:ea395616348c 205 pc.printf("speed: %f",get_avg_speed(num_samples_small, delay_small));
mawk2311 5:61a0a21134f7 206
mawk2311 5:61a0a21134f7 207 while(1){
mawk2311 7:ea395616348c 208 velocity_control(0.3f, TUNING_CONSTANT_30);
mawk2311 5:61a0a21134f7 209 }
mawk2311 5:61a0a21134f7 210
mawk2311 5:61a0a21134f7 211 //break;
ericoneill 0:d328ecb3fbb1 212 case '5':
mawk2311 11:d07a4a683289 213 motor1.pulsewidth(.0025*.5);
mawk2311 11:d07a4a683289 214 motor2.pulsewidth(.0025*.5);
mawk2311 7:ea395616348c 215 pc.printf("\n\r50% \n\r");
ericoneill 4:263bddc51c0f 216 wait(.5);
mawk2311 7:ea395616348c 217 pc.printf("speed: %f",get_avg_speed(num_samples_small, delay_small));
mawk2311 5:61a0a21134f7 218
mawk2311 5:61a0a21134f7 219 while(1){
mawk2311 7:ea395616348c 220 velocity_control(0.5f, TUNING_CONSTANT_50);
mawk2311 5:61a0a21134f7 221 }
mawk2311 5:61a0a21134f7 222
mawk2311 5:61a0a21134f7 223 //break;
mawk2311 7:ea395616348c 224 case 'a':
mawk2311 7:ea395616348c 225 pc.printf("\n\rGet average velocity of which duty cycle?\n\r");
mawk2311 7:ea395616348c 226 choice = pc.getc();
mawk2311 7:ea395616348c 227 pc.putc(choice);
mawk2311 7:ea395616348c 228
mawk2311 7:ea395616348c 229 switch(choice){
mawk2311 7:ea395616348c 230
mawk2311 7:ea395616348c 231 case '2':
mawk2311 11:d07a4a683289 232 motor1.pulsewidth(.0025*0.2f);
mawk2311 11:d07a4a683289 233 motor2.pulsewidth(.0025*0.2f);
mawk2311 7:ea395616348c 234 pc.printf("\n\rLongterm average speed at 20 Duty Cycle: %f\n\r", get_avg_speed(num_samples_large, delay_large));
mawk2311 7:ea395616348c 235 break;
mawk2311 7:ea395616348c 236
mawk2311 7:ea395616348c 237 case '3':
mawk2311 11:d07a4a683289 238 motor1.pulsewidth(.0025*0.3f);
mawk2311 11:d07a4a683289 239 motor2.pulsewidth(.0025*0.3f);
mawk2311 7:ea395616348c 240 pc.printf("\n\rLongterm average speed at 30 Duty Cycle: %f\n\r", get_avg_speed(num_samples_large, delay_large));
mawk2311 7:ea395616348c 241 break;
mawk2311 7:ea395616348c 242
mawk2311 7:ea395616348c 243 case '5':
mawk2311 11:d07a4a683289 244 motor1.pulsewidth(.0025*0.5f);
mawk2311 11:d07a4a683289 245 motor2.pulsewidth(.0025*0.5f);
mawk2311 7:ea395616348c 246 pc.printf("\n\rLongterm average speed at 50 Duty Cycle: %f\n\r", get_avg_speed(num_samples_large, delay_large));
mawk2311 7:ea395616348c 247 break;
mawk2311 7:ea395616348c 248
mawk2311 7:ea395616348c 249 default:
mawk2311 7:ea395616348c 250 break;
mawk2311 7:ea395616348c 251 }
mawk2311 7:ea395616348c 252 break;
mawk2311 7:ea395616348c 253
ericoneill 0:d328ecb3fbb1 254 default:
mawk2311 11:d07a4a683289 255 motor1.pulsewidth(.0025*0);
mawk2311 11:d07a4a683289 256 motor2.pulsewidth(.0025*0);
ericoneill 2:30ebae0d3e17 257 pc.printf("default\n\r");
ericoneill 0:d328ecb3fbb1 258 break;
ericoneill 0:d328ecb3fbb1 259 }
ericoneill 3:7eaf505f811e 260
ericoneill 0:d328ecb3fbb1 261 //servo_sweep();
ericoneill 0:d328ecb3fbb1 262 }
ericoneill 0:d328ecb3fbb1 263 }