E=MC / Mbed 2 deprecated e=mc

.

Dependencies:   Servo mbed

main.cpp@6:78a2c2a7f39e, 2015-03-02 (annotated)

Committer:
ericoneill
Date:
Mon Mar 02 19:38:38 2015 +0000
Revision:
6:78a2c2a7f39e
Parent:
5:61a0a21134f7
Child:
7:ea395616348c
Stall checking

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
ericoneill 6:78a2c2a7f39e 100 float avg_speed;
ericoneill 6:78a2c2a7f39e 101 float stall_check;
mawk2311 5:61a0a21134f7 102
ericoneill 2:30ebae0d3e17 103 char choice = pc.getc();
mawk2311 5:61a0a21134f7 104 pc.putc(choice);
mawk2311 5:61a0a21134f7 105
ericoneill 2:30ebae0d3e17 106 switch(choice){
ericoneill 0:d328ecb3fbb1 107 case '0':
ericoneill 0:d328ecb3fbb1 108 motor.pulsewidth(0.0);
ericoneill 2:30ebae0d3e17 109 pc.printf("0% \n\r");
ericoneill 4:263bddc51c0f 110
ericoneill 0:d328ecb3fbb1 111 break;
ericoneill 0:d328ecb3fbb1 112 case '1':
ericoneill 0:d328ecb3fbb1 113 motor.pulsewidth(.0025);
ericoneill 2:30ebae0d3e17 114 pc.printf("100% \n\r");
ericoneill 4:263bddc51c0f 115 wait(.5);
ericoneill 4:263bddc51c0f 116 pc.printf("speed: %f",get_speed());
mawk2311 5:61a0a21134f7 117
ericoneill 4:263bddc51c0f 118 break;
ericoneill 4:263bddc51c0f 119 case '2':
ericoneill 4:263bddc51c0f 120 motor.pulsewidth(.0025*.2);
ericoneill 4:263bddc51c0f 121 pc.printf("20% \n\r");
mawk2311 5:61a0a21134f7 122 wait(.5);
mawk2311 5:61a0a21134f7 123 float tuning_val = 1;
mawk2311 5:61a0a21134f7 124 pc.printf("speed: %f\n\rtuning val: %f\n\r", get_avg_speed());
mawk2311 5:61a0a21134f7 125
mawk2311 5:61a0a21134f7 126 while(1){
mawk2311 5:61a0a21134f7 127
mawk2311 5:61a0a21134f7 128 avg_speed = get_avg_speed();
ericoneill 6:78a2c2a7f39e 129 if (avg_speed == stall_check) {
ericoneill 6:78a2c2a7f39e 130 avg_speed = 0;
ericoneill 6:78a2c2a7f39e 131 tuning_val += .1;
ericoneill 6:78a2c2a7f39e 132 } else if((avg_speed - TUNING_CONSTANT_20) > 0.5f){
mawk2311 5:61a0a21134f7 133 tuning_val -= .1;
ericoneill 6:78a2c2a7f39e 134 stall_check = avg_speed;
mawk2311 5:61a0a21134f7 135 } else if (avg_speed - TUNING_CONSTANT_20 < -0.5f){
mawk2311 5:61a0a21134f7 136 tuning_val += .1;
ericoneill 6:78a2c2a7f39e 137 stall_check = avg_speed;
mawk2311 5:61a0a21134f7 138 } else {
mawk2311 5:61a0a21134f7 139 tuning_val = 1;
ericoneill 6:78a2c2a7f39e 140 stall_check = avg_speed;
mawk2311 5:61a0a21134f7 141 }
mawk2311 5:61a0a21134f7 142 motor.pulsewidth(.0025 * .2 * tuning_val);
ericoneill 6:78a2c2a7f39e 143
ericoneill 6:78a2c2a7f39e 144 pc.printf("speed: %f\n\rtuning val: %f\n\r PWM : ", avg_speed, tuning_val);
mawk2311 5:61a0a21134f7 145 wait(.5);
mawk2311 5:61a0a21134f7 146 }
mawk2311 5:61a0a21134f7 147
mawk2311 5:61a0a21134f7 148 //break;
mawk2311 5:61a0a21134f7 149
ericoneill 0:d328ecb3fbb1 150 case '3':
ericoneill 0:d328ecb3fbb1 151 motor.pulsewidth(.0025*.3);
ericoneill 2:30ebae0d3e17 152 pc.printf("30% \n\r");
ericoneill 4:263bddc51c0f 153 wait(.5);
mawk2311 5:61a0a21134f7 154 pc.printf("speed: %f",get_avg_speed());
mawk2311 5:61a0a21134f7 155
mawk2311 5:61a0a21134f7 156 while(1){
mawk2311 5:61a0a21134f7 157
mawk2311 5:61a0a21134f7 158 avg_speed = get_avg_speed();
mawk2311 5:61a0a21134f7 159
mawk2311 5:61a0a21134f7 160 if((avg_speed - TUNING_CONSTANT_30) > 0.5f){
mawk2311 5:61a0a21134f7 161 tuning_val -= .1;
mawk2311 5:61a0a21134f7 162 } else if (avg_speed - TUNING_CONSTANT_30 < -0.5f){
mawk2311 5:61a0a21134f7 163 tuning_val += .1;
mawk2311 5:61a0a21134f7 164 } else {
mawk2311 5:61a0a21134f7 165 tuning_val = 1;
mawk2311 5:61a0a21134f7 166 }
mawk2311 5:61a0a21134f7 167 motor.pulsewidth(.0025 * .3 * tuning_val);
mawk2311 5:61a0a21134f7 168 pc.printf("speed: %f\n\rtuning val: %f\n\r", avg_speed, tuning_val);
mawk2311 5:61a0a21134f7 169 wait(.5);
mawk2311 5:61a0a21134f7 170 }
mawk2311 5:61a0a21134f7 171
mawk2311 5:61a0a21134f7 172 //break;
ericoneill 0:d328ecb3fbb1 173 case '5':
ericoneill 0:d328ecb3fbb1 174 motor.pulsewidth(.0025*.5);
ericoneill 2:30ebae0d3e17 175 pc.printf("50% \n\r");
ericoneill 4:263bddc51c0f 176 wait(.5);
mawk2311 5:61a0a21134f7 177 pc.printf("speed: %f",get_avg_speed());
mawk2311 5:61a0a21134f7 178
mawk2311 5:61a0a21134f7 179 while(1){
mawk2311 5:61a0a21134f7 180
mawk2311 5:61a0a21134f7 181 avg_speed = get_avg_speed();
mawk2311 5:61a0a21134f7 182
mawk2311 5:61a0a21134f7 183 if((avg_speed - TUNING_CONSTANT_50) > 0.5f){
mawk2311 5:61a0a21134f7 184 tuning_val -= .1;
mawk2311 5:61a0a21134f7 185 } else if (avg_speed - TUNING_CONSTANT_50 < -0.5f){
mawk2311 5:61a0a21134f7 186 tuning_val += .1;
mawk2311 5:61a0a21134f7 187 } else {
mawk2311 5:61a0a21134f7 188 tuning_val = 1;
mawk2311 5:61a0a21134f7 189 }
mawk2311 5:61a0a21134f7 190 motor.pulsewidth(.0025 * .5 * tuning_val);
mawk2311 5:61a0a21134f7 191 pc.printf("speed: %f\n\rtuning val: %f\n\r", avg_speed, tuning_val);
mawk2311 5:61a0a21134f7 192 wait(.5);
mawk2311 5:61a0a21134f7 193 }
mawk2311 5:61a0a21134f7 194
mawk2311 5:61a0a21134f7 195 //break;
ericoneill 0:d328ecb3fbb1 196 default:
ericoneill 0:d328ecb3fbb1 197 motor.pulsewidth(.0025*.3);
ericoneill 2:30ebae0d3e17 198 pc.printf("default\n\r");
ericoneill 0:d328ecb3fbb1 199 break;
ericoneill 0:d328ecb3fbb1 200 }
ericoneill 3:7eaf505f811e 201
ericoneill 0:d328ecb3fbb1 202 //servo_sweep();
ericoneill 0:d328ecb3fbb1 203 }
ericoneill 0:d328ecb3fbb1 204 }