E=MC / Mbed 2 deprecated e=mc

.

Dependencies:   Servo mbed

main.cpp@5:61a0a21134f7, 2015-02-27 (annotated)

Committer:
mawk2311
Date:
Fri Feb 27 06:36:42 2015 +0000
Revision:
5:61a0a21134f7
Parent:
4:263bddc51c0f
Child:
6:78a2c2a7f39e
Got feedback control working for 20,30,50 duty cycle

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);
ericoneill 0:d328ecb3fbb1 5 PwmOut motor(PTA4);
ericoneill 0:d328ecb3fbb1 6 Serial pc(USBTX, USBRX); // tx, rx
ericoneill 3:7eaf505f811e 7
ericoneill 3:7eaf505f811e 8 // encoder setup and variables
ericoneill 3:7eaf505f811e 9 InterruptIn interrupt(PTA13);
ericoneill 4:263bddc51c0f 10
ericoneill 4:263bddc51c0f 11 int interval1=0;
ericoneill 4:263bddc51c0f 12 int interval2=0;
ericoneill 4:263bddc51c0f 13 int interval3=0;
ericoneill 4:263bddc51c0f 14 int avg_interval=0;
ericoneill 4:263bddc51c0f 15 int lastchange1 = 0;
ericoneill 4:263bddc51c0f 16 int lastchange2 = 0;
ericoneill 4:263bddc51c0f 17 int lastchange3 = 0;
ericoneill 4:263bddc51c0f 18 int lastchange4 = 0;
mawk2311 5:61a0a21134f7 19
mawk2311 5:61a0a21134f7 20 float avg_speed_list [100];
mawk2311 5:61a0a21134f7 21 float small_avg_speed_list [10];
mawk2311 5:61a0a21134f7 22
ericoneill 1:8e5821dec0f7 23 Timer t;
ericoneill 4:263bddc51c0f 24 //constant *(PWM-expected)
mawk2311 5:61a0a21134f7 25 const float TUNING_CONSTANT_20 = 3.697;
mawk2311 5:61a0a21134f7 26 const float TUNING_CONSTANT_30 = 5.607;
mawk2311 5:61a0a21134f7 27 const float TUNING_CONSTANT_50 = 6.880;
ericoneill 4:263bddc51c0f 28 const float PI = 3.14159;
ericoneill 4:263bddc51c0f 29 const float WHEEL_CIRCUMFERENCE = .05*PI;
ericoneill 0:d328ecb3fbb1 30
ericoneill 4:263bddc51c0f 31 float get_speed(){
ericoneill 4:263bddc51c0f 32 float revPerSec = (1.0f/((float)avg_interval*.000001))*.25f;
ericoneill 4:263bddc51c0f 33 float linearSpeed = revPerSec * WHEEL_CIRCUMFERENCE;
ericoneill 4:263bddc51c0f 34 return linearSpeed;
ericoneill 4:263bddc51c0f 35
ericoneill 4:263bddc51c0f 36 }
mawk2311 5:61a0a21134f7 37
mawk2311 5:61a0a21134f7 38 float get_avg_speed() {
mawk2311 5:61a0a21134f7 39
mawk2311 5:61a0a21134f7 40 float avg_avg_speed = 0;
mawk2311 5:61a0a21134f7 41
mawk2311 5:61a0a21134f7 42 for (int c = 0; c < 10; c++) {
mawk2311 5:61a0a21134f7 43 small_avg_speed_list[c] = get_speed();
mawk2311 5:61a0a21134f7 44 wait(.05);
mawk2311 5:61a0a21134f7 45 }
mawk2311 5:61a0a21134f7 46
mawk2311 5:61a0a21134f7 47 for (int c = 0; c < 10; c++) {
mawk2311 5:61a0a21134f7 48 avg_avg_speed += small_avg_speed_list[c];
mawk2311 5:61a0a21134f7 49 }
mawk2311 5:61a0a21134f7 50
mawk2311 5:61a0a21134f7 51 return avg_avg_speed/10.0f;
mawk2311 5:61a0a21134f7 52 }
mawk2311 5:61a0a21134f7 53
ericoneill 0:d328ecb3fbb1 54 void servo_sweep(){
ericoneill 0:d328ecb3fbb1 55 for(float p = 0.001; p<0.002; p+=0.0001){
ericoneill 0:d328ecb3fbb1 56 servo.pulsewidth(p);
ericoneill 0:d328ecb3fbb1 57 wait(0.5);
ericoneill 0:d328ecb3fbb1 58 }
ericoneill 0:d328ecb3fbb1 59 }
ericoneill 3:7eaf505f811e 60 void fallInterrupt(){
ericoneill 4:263bddc51c0f 61
ericoneill 4:263bddc51c0f 62 int time = t.read_us();
ericoneill 4:263bddc51c0f 63 interval1 = time - lastchange2;
ericoneill 4:263bddc51c0f 64 interval2 = lastchange1-lastchange3;
ericoneill 4:263bddc51c0f 65 interval3 = lastchange2 - lastchange4;
ericoneill 4:263bddc51c0f 66 avg_interval = (interval1 + interval2 + interval3)/3;
ericoneill 4:263bddc51c0f 67
ericoneill 4:263bddc51c0f 68 lastchange4 = lastchange3;
ericoneill 4:263bddc51c0f 69 lastchange3 = lastchange2;
ericoneill 4:263bddc51c0f 70 lastchange2 = lastchange1;
ericoneill 4:263bddc51c0f 71 lastchange1 = time;
ericoneill 4:263bddc51c0f 72 //pc.printf("dark to light time : %d\n\r", interval);
ericoneill 4:263bddc51c0f 73 //pc.printf("fall");
ericoneill 3:7eaf505f811e 74 }
ericoneill 3:7eaf505f811e 75 void riseInterrupt(){
ericoneill 4:263bddc51c0f 76 int time = t.read_us();
ericoneill 4:263bddc51c0f 77 interval1 = time - lastchange2;
ericoneill 4:263bddc51c0f 78 interval2 = lastchange1-lastchange3;
ericoneill 4:263bddc51c0f 79 interval3 = lastchange2 - lastchange4;
ericoneill 4:263bddc51c0f 80 avg_interval = (interval1 + interval2 + interval3)/3;
ericoneill 4:263bddc51c0f 81
ericoneill 4:263bddc51c0f 82 lastchange4 = lastchange3;
ericoneill 4:263bddc51c0f 83 lastchange3 = lastchange2;
ericoneill 4:263bddc51c0f 84 lastchange2 = lastchange1;
ericoneill 4:263bddc51c0f 85 lastchange1 = time;
ericoneill 4:263bddc51c0f 86 //pc.printf("light to dark time : %d\n\r", interval);
ericoneill 4:263bddc51c0f 87 //pc.printf("rise");
ericoneill 3:7eaf505f811e 88 }
ericoneill 3:7eaf505f811e 89
ericoneill 0:d328ecb3fbb1 90 int main() {
ericoneill 0:d328ecb3fbb1 91 servo.period(0.005);
ericoneill 0:d328ecb3fbb1 92 motor.period(.0025);
ericoneill 3:7eaf505f811e 93 interrupt.fall(&fallInterrupt);
ericoneill 3:7eaf505f811e 94 interrupt.rise(&riseInterrupt);
ericoneill 3:7eaf505f811e 95
ericoneill 1:8e5821dec0f7 96
ericoneill 1:8e5821dec0f7 97 t.start();
ericoneill 4:263bddc51c0f 98 while(1){
ericoneill 2:30ebae0d3e17 99
mawk2311 5:61a0a21134f7 100 float avg_speed = 0;
mawk2311 5:61a0a21134f7 101
ericoneill 2:30ebae0d3e17 102 char choice = pc.getc();
mawk2311 5:61a0a21134f7 103 pc.putc(choice);
mawk2311 5:61a0a21134f7 104
ericoneill 2:30ebae0d3e17 105 switch(choice){
ericoneill 0:d328ecb3fbb1 106 case '0':
ericoneill 0:d328ecb3fbb1 107 motor.pulsewidth(0.0);
ericoneill 2:30ebae0d3e17 108 pc.printf("0% \n\r");
ericoneill 4:263bddc51c0f 109
ericoneill 0:d328ecb3fbb1 110 break;
ericoneill 0:d328ecb3fbb1 111 case '1':
ericoneill 0:d328ecb3fbb1 112 motor.pulsewidth(.0025);
ericoneill 2:30ebae0d3e17 113 pc.printf("100% \n\r");
ericoneill 4:263bddc51c0f 114 wait(.5);
ericoneill 4:263bddc51c0f 115 pc.printf("speed: %f",get_speed());
mawk2311 5:61a0a21134f7 116
ericoneill 4:263bddc51c0f 117 break;
ericoneill 4:263bddc51c0f 118 case '2':
ericoneill 4:263bddc51c0f 119 motor.pulsewidth(.0025*.2);
ericoneill 4:263bddc51c0f 120 pc.printf("20% \n\r");
mawk2311 5:61a0a21134f7 121 wait(.5);
mawk2311 5:61a0a21134f7 122 float tuning_val = 1;
mawk2311 5:61a0a21134f7 123 pc.printf("speed: %f\n\rtuning val: %f\n\r", get_avg_speed());
mawk2311 5:61a0a21134f7 124
mawk2311 5:61a0a21134f7 125 while(1){
mawk2311 5:61a0a21134f7 126
mawk2311 5:61a0a21134f7 127 avg_speed = get_avg_speed();
mawk2311 5:61a0a21134f7 128
mawk2311 5:61a0a21134f7 129 if((avg_speed - TUNING_CONSTANT_20) > 0.5f){
mawk2311 5:61a0a21134f7 130 tuning_val -= .1;
mawk2311 5:61a0a21134f7 131 } else if (avg_speed - TUNING_CONSTANT_20 < -0.5f){
mawk2311 5:61a0a21134f7 132 tuning_val += .1;
mawk2311 5:61a0a21134f7 133 } else {
mawk2311 5:61a0a21134f7 134 tuning_val = 1;
mawk2311 5:61a0a21134f7 135 }
mawk2311 5:61a0a21134f7 136 motor.pulsewidth(.0025 * .2 * tuning_val);
mawk2311 5:61a0a21134f7 137 pc.printf("speed: %f\n\rtuning val: %f\n\r", avg_speed, tuning_val);
mawk2311 5:61a0a21134f7 138 wait(.5);
mawk2311 5:61a0a21134f7 139 }
mawk2311 5:61a0a21134f7 140
mawk2311 5:61a0a21134f7 141 //break;
mawk2311 5:61a0a21134f7 142
ericoneill 0:d328ecb3fbb1 143 case '3':
ericoneill 0:d328ecb3fbb1 144 motor.pulsewidth(.0025*.3);
ericoneill 2:30ebae0d3e17 145 pc.printf("30% \n\r");
ericoneill 4:263bddc51c0f 146 wait(.5);
mawk2311 5:61a0a21134f7 147 pc.printf("speed: %f",get_avg_speed());
mawk2311 5:61a0a21134f7 148
mawk2311 5:61a0a21134f7 149 while(1){
mawk2311 5:61a0a21134f7 150
mawk2311 5:61a0a21134f7 151 avg_speed = get_avg_speed();
mawk2311 5:61a0a21134f7 152
mawk2311 5:61a0a21134f7 153 if((avg_speed - TUNING_CONSTANT_30) > 0.5f){
mawk2311 5:61a0a21134f7 154 tuning_val -= .1;
mawk2311 5:61a0a21134f7 155 } else if (avg_speed - TUNING_CONSTANT_30 < -0.5f){
mawk2311 5:61a0a21134f7 156 tuning_val += .1;
mawk2311 5:61a0a21134f7 157 } else {
mawk2311 5:61a0a21134f7 158 tuning_val = 1;
mawk2311 5:61a0a21134f7 159 }
mawk2311 5:61a0a21134f7 160 motor.pulsewidth(.0025 * .3 * tuning_val);
mawk2311 5:61a0a21134f7 161 pc.printf("speed: %f\n\rtuning val: %f\n\r", avg_speed, tuning_val);
mawk2311 5:61a0a21134f7 162 wait(.5);
mawk2311 5:61a0a21134f7 163 }
mawk2311 5:61a0a21134f7 164
mawk2311 5:61a0a21134f7 165 //break;
ericoneill 0:d328ecb3fbb1 166 case '5':
ericoneill 0:d328ecb3fbb1 167 motor.pulsewidth(.0025*.5);
ericoneill 2:30ebae0d3e17 168 pc.printf("50% \n\r");
ericoneill 4:263bddc51c0f 169 wait(.5);
mawk2311 5:61a0a21134f7 170 pc.printf("speed: %f",get_avg_speed());
mawk2311 5:61a0a21134f7 171
mawk2311 5:61a0a21134f7 172 while(1){
mawk2311 5:61a0a21134f7 173
mawk2311 5:61a0a21134f7 174 avg_speed = get_avg_speed();
mawk2311 5:61a0a21134f7 175
mawk2311 5:61a0a21134f7 176 if((avg_speed - TUNING_CONSTANT_50) > 0.5f){
mawk2311 5:61a0a21134f7 177 tuning_val -= .1;
mawk2311 5:61a0a21134f7 178 } else if (avg_speed - TUNING_CONSTANT_50 < -0.5f){
mawk2311 5:61a0a21134f7 179 tuning_val += .1;
mawk2311 5:61a0a21134f7 180 } else {
mawk2311 5:61a0a21134f7 181 tuning_val = 1;
mawk2311 5:61a0a21134f7 182 }
mawk2311 5:61a0a21134f7 183 motor.pulsewidth(.0025 * .5 * tuning_val);
mawk2311 5:61a0a21134f7 184 pc.printf("speed: %f\n\rtuning val: %f\n\r", avg_speed, tuning_val);
mawk2311 5:61a0a21134f7 185 wait(.5);
mawk2311 5:61a0a21134f7 186 }
mawk2311 5:61a0a21134f7 187
mawk2311 5:61a0a21134f7 188 //break;
ericoneill 0:d328ecb3fbb1 189 default:
ericoneill 0:d328ecb3fbb1 190 motor.pulsewidth(.0025*.3);
ericoneill 2:30ebae0d3e17 191 pc.printf("default\n\r");
ericoneill 0:d328ecb3fbb1 192 break;
ericoneill 0:d328ecb3fbb1 193 }
ericoneill 3:7eaf505f811e 194
ericoneill 0:d328ecb3fbb1 195 //servo_sweep();
ericoneill 0:d328ecb3fbb1 196 }
ericoneill 0:d328ecb3fbb1 197 }