CashComplete2.0 / Mbed 2 deprecated TEST_SM_SPEED

Test the speed of sensor module PID controlled speed. When note is passed the optical sensors data is collected and shared via SWO

Dependencies:   mbed PID SWO

main.cpp@0:fc81857d8067, 2018-11-30 (annotated)

Committer:
andjoh
Date:
Fri Nov 30 10:22:36 2018 +0000
Revision:
0:fc81857d8067
Check speed of sensor module

Who changed what in which revision?

UserRevisionLine numberNew contents of line
andjoh 0:fc81857d8067 1 #include "mbed.h"
andjoh 0:fc81857d8067 2 #include "SWO.h"
andjoh 0:fc81857d8067 3 #include "PID.h"
andjoh 0:fc81857d8067 4
andjoh 0:fc81857d8067 5 #define RATE 5 //interval PID calculation performed every 5th milliseconds.
andjoh 0:fc81857d8067 6
andjoh 0:fc81857d8067 7
andjoh 0:fc81857d8067 8
andjoh 0:fc81857d8067 9 DigitalOut LED_GREEN(PA_4);
andjoh 0:fc81857d8067 10 DigitalOut LED_RED(PA_5);
andjoh 0:fc81857d8067 11 DigitalOut LED_ENABLE(PB_5);
andjoh 0:fc81857d8067 12
andjoh 0:fc81857d8067 13 //optical sensors
andjoh 0:fc81857d8067 14 DigitalIn SCANNER_INPUT(PC_10);
andjoh 0:fc81857d8067 15 DigitalIn SCANNER_OUTPUT(PC_1);
andjoh 0:fc81857d8067 16 DigitalIn SWITCH_INPUT(PC_2);
andjoh 0:fc81857d8067 17 DigitalIn REJECT_INPUT(PA_7);
andjoh 0:fc81857d8067 18 DigitalIn REJECT_OUTPUT(PA_6);
andjoh 0:fc81857d8067 19 DigitalIn STORAGE_INPUT(PC_3);
andjoh 0:fc81857d8067 20
andjoh 0:fc81857d8067 21 int opticalSensors[6] = {0}; //list to store the values from optical sensors
andjoh 0:fc81857d8067 22 int lastValueOpticalSensors[6] = {0}; //list to store the previous values from optical sensors
andjoh 0:fc81857d8067 23 float sensorNoteRunningTimes[10] = {0}; //The times (in us) when note hit the sensors are stored in this array
andjoh 0:fc81857d8067 24 int pulseCountBetweenSensors[10] = {0}; //The counted number encoder pulses between sensors
andjoh 0:fc81857d8067 25 int noteInSystem = 0;
andjoh 0:fc81857d8067 26 long lastPulseTime = 0;
andjoh 0:fc81857d8067 27 long maxTimeBetweenPulses = 0;
andjoh 0:fc81857d8067 28 long minTimeBetweenPulses = 10000000;
andjoh 0:fc81857d8067 29 long timeBetweenPulses = 0;
andjoh 0:fc81857d8067 30 double currentSpeed = 0; //speed in mm/ms
andjoh 0:fc81857d8067 31 long lastControl = 0;
andjoh 0:fc81857d8067 32
andjoh 0:fc81857d8067 33 PID controller(4.0, 0.0, 0.0, RATE);
andjoh 0:fc81857d8067 34
andjoh 0:fc81857d8067 35
andjoh 0:fc81857d8067 36 int AtoB_dist = 357; //distance from SCANNER_INPUT to SCANNER_OUTPUT counted pulses: 372
andjoh 0:fc81857d8067 37 int BtoC_dist = 81; //distance from SCANNER_OUTPUT to SWITCH_INPUT counted pulses: 82
andjoh 0:fc81857d8067 38 int CtoD_dist = 201; //distance from SWITCH_INPUT to REJECT_INPUT counted pulses: 203
andjoh 0:fc81857d8067 39 int DtoE_dist = 158; //distance from REJECT_INPUT to REJECT_OUTPUT counted pulses: 156
andjoh 0:fc81857d8067 40 int Tot_dist = 797; //distance from SCANNER_INPUT to REJECT_OUTPUT counted pulses: 813
andjoh 0:fc81857d8067 41
andjoh 0:fc81857d8067 42 int TenEURnoteLength = 127;
andjoh 0:fc81857d8067 43 int FiftyEURnoteLength = 140;
andjoh 0:fc81857d8067 44 int TwentyEURnoteLength = 120;
andjoh 0:fc81857d8067 45
andjoh 0:fc81857d8067 46 //encoders
andjoh 0:fc81857d8067 47 InterruptIn MAIN_ENC(PA_8);
andjoh 0:fc81857d8067 48 //InterruptIn STORAGE_ENC(PA_15);
andjoh 0:fc81857d8067 49 //InterruptIn SWITCH_ENC(PB_4);
andjoh 0:fc81857d8067 50 //InterruptIn ROUTER_ENC(PB_6);
andjoh 0:fc81857d8067 51
andjoh 0:fc81857d8067 52
andjoh 0:fc81857d8067 53 //Motors
andjoh 0:fc81857d8067 54 PwmOut MAIN_PWM(PC_7); //PWM to Main Drive
andjoh 0:fc81857d8067 55 DigitalOut MAIN_DIR(PC_0); //Direction of main drive
andjoh 0:fc81857d8067 56
andjoh 0:fc81857d8067 57 PwmOut SWITCH_PWM(PB_14); //PWM to Switch
andjoh 0:fc81857d8067 58 DigitalOut SWITCH_DIR(PB_15); //Direction of Switch
andjoh 0:fc81857d8067 59
andjoh 0:fc81857d8067 60 PwmOut ROUTER_PWM(PC_8); //PWM to Router
andjoh 0:fc81857d8067 61 DigitalOut ROUTER_DIR(PB_7); //Direction of Router
andjoh 0:fc81857d8067 62
andjoh 0:fc81857d8067 63 PwmOut STORAGE_PWM(PC_6); //PWM to Storage
andjoh 0:fc81857d8067 64 DigitalOut STORAGE_DIR(PC_9); //Direction of Storage
andjoh 0:fc81857d8067 65
andjoh 0:fc81857d8067 66
andjoh 0:fc81857d8067 67
andjoh 0:fc81857d8067 68 uint32_t blinkTime_ms = 0;
andjoh 0:fc81857d8067 69 uint32_t reportTimer = 0;
andjoh 0:fc81857d8067 70 uint32_t pulses = 0;
andjoh 0:fc81857d8067 71
andjoh 0:fc81857d8067 72
andjoh 0:fc81857d8067 73 Timer t;
andjoh 0:fc81857d8067 74 //PID controller(1, 0, 0, RATE); //Kc, Ti, Td, interval
andjoh 0:fc81857d8067 75
andjoh 0:fc81857d8067 76
andjoh 0:fc81857d8067 77 Serial pc(USBTX, USBRX);
andjoh 0:fc81857d8067 78 SWO_Channel swo("channel");
andjoh 0:fc81857d8067 79
andjoh 0:fc81857d8067 80 void green_blink(void)
andjoh 0:fc81857d8067 81 {
andjoh 0:fc81857d8067 82 if(t.read_ms() - blinkTime_ms > 200) {
andjoh 0:fc81857d8067 83 LED_RED = 0;
andjoh 0:fc81857d8067 84 LED_GREEN = !LED_GREEN;
andjoh 0:fc81857d8067 85 blinkTime_ms = t.read_ms();
andjoh 0:fc81857d8067 86 }
andjoh 0:fc81857d8067 87 }
andjoh 0:fc81857d8067 88 void red_blink(void)
andjoh 0:fc81857d8067 89 {
andjoh 0:fc81857d8067 90 if(t.read_ms() - blinkTime_ms > 200) {
andjoh 0:fc81857d8067 91 LED_GREEN = 0;
andjoh 0:fc81857d8067 92 LED_RED = !LED_RED;
andjoh 0:fc81857d8067 93 blinkTime_ms = t.read_ms();
andjoh 0:fc81857d8067 94 }
andjoh 0:fc81857d8067 95 }
andjoh 0:fc81857d8067 96
andjoh 0:fc81857d8067 97 void reportNoteThroughSensorLengths()
andjoh 0:fc81857d8067 98 {
andjoh 0:fc81857d8067 99
andjoh 0:fc81857d8067 100 swo.printf("\n");
andjoh 0:fc81857d8067 101
andjoh 0:fc81857d8067 102 //Time for note to travel through first sensor
andjoh 0:fc81857d8067 103 long time = (sensorNoteRunningTimes[1] - sensorNoteRunningTimes[0]); //us
andjoh 0:fc81857d8067 104 swo.printf("Time for note to pass 1th sensor (us): %d\r", time);
andjoh 0:fc81857d8067 105 //int length = TwentyEURnoteLength;
andjoh 0:fc81857d8067 106 int countedPulses = pulseCountBetweenSensors[1] - pulseCountBetweenSensors[0];
andjoh 0:fc81857d8067 107 swo.printf(" Counted pulses: %d\r\n", countedPulses);
andjoh 0:fc81857d8067 108
andjoh 0:fc81857d8067 109 //Time for note to travel through second sensor
andjoh 0:fc81857d8067 110 time = (sensorNoteRunningTimes[3] - sensorNoteRunningTimes[2]); //us
andjoh 0:fc81857d8067 111 swo.printf("Time for note to pass 2th sensor (us): %d\r\n", time);
andjoh 0:fc81857d8067 112 countedPulses = pulseCountBetweenSensors[3] - pulseCountBetweenSensors[2];
andjoh 0:fc81857d8067 113 swo.printf(" Counted pulses: %d\r\n", countedPulses);
andjoh 0:fc81857d8067 114
andjoh 0:fc81857d8067 115 //Time for note to travel through third sensor
andjoh 0:fc81857d8067 116 time = (sensorNoteRunningTimes[5] - sensorNoteRunningTimes[4]); //us
andjoh 0:fc81857d8067 117 swo.printf("Time for note to pass 3th sensor (us): %d\r\n", time);
andjoh 0:fc81857d8067 118 countedPulses = pulseCountBetweenSensors[5] - pulseCountBetweenSensors[4];
andjoh 0:fc81857d8067 119 swo.printf(" Counted pulses: %d\r\n", countedPulses);
andjoh 0:fc81857d8067 120
andjoh 0:fc81857d8067 121 //Time for note to travel through fourth sensor
andjoh 0:fc81857d8067 122 time = (sensorNoteRunningTimes[7] - sensorNoteRunningTimes[6]); //us
andjoh 0:fc81857d8067 123 swo.printf("Time for note to pass 4th sensor (us): %d\r\n", time);
andjoh 0:fc81857d8067 124 countedPulses = pulseCountBetweenSensors[7] - pulseCountBetweenSensors[6];
andjoh 0:fc81857d8067 125 swo.printf(" Counted pulses: %d\r\n", countedPulses);
andjoh 0:fc81857d8067 126
andjoh 0:fc81857d8067 127 //Time for note to travel through fifth sensor
andjoh 0:fc81857d8067 128 time = (sensorNoteRunningTimes[9] - sensorNoteRunningTimes[8]); //us
andjoh 0:fc81857d8067 129 swo.printf("Time for note to pass 5th sensor (us): %d\r\n", time);
andjoh 0:fc81857d8067 130 countedPulses = pulseCountBetweenSensors[9] - pulseCountBetweenSensors[8];
andjoh 0:fc81857d8067 131 swo.printf(" Counted pulses: %d\r\n", countedPulses);
andjoh 0:fc81857d8067 132
andjoh 0:fc81857d8067 133 swo.printf("\n");
andjoh 0:fc81857d8067 134 swo.printf("\n");
andjoh 0:fc81857d8067 135
andjoh 0:fc81857d8067 136 }
andjoh 0:fc81857d8067 137
andjoh 0:fc81857d8067 138 void report(void)
andjoh 0:fc81857d8067 139 {
andjoh 0:fc81857d8067 140 //uint32_t now = t.read_ms();
andjoh 0:fc81857d8067 141
andjoh 0:fc81857d8067 142 /*
andjoh 0:fc81857d8067 143 swo.printf("Optical sensor status: ");
andjoh 0:fc81857d8067 144 for(int j = 0; j <=5 ; j++) {
andjoh 0:fc81857d8067 145 swo.printf("%d ", opticalSensors[j]);
andjoh 0:fc81857d8067 146 }
andjoh 0:fc81857d8067 147 swo.printf("\n");
andjoh 0:fc81857d8067 148 */
andjoh 0:fc81857d8067 149
andjoh 0:fc81857d8067 150 long time = (sensorNoteRunningTimes[2] - sensorNoteRunningTimes[0]) / 1000; //ms
andjoh 0:fc81857d8067 151
andjoh 0:fc81857d8067 152 //swo.printf("Time between first two sensors (ms): ");
andjoh 0:fc81857d8067 153 //swo.printf("%d ", time);
andjoh 0:fc81857d8067 154 //swo.printf("\n");
andjoh 0:fc81857d8067 155
andjoh 0:fc81857d8067 156 //V = S / T
andjoh 0:fc81857d8067 157 float V = 1000 * AtoB_dist / time; //(mm/s)
andjoh 0:fc81857d8067 158
andjoh 0:fc81857d8067 159 swo.printf("Speed between first two sensors (mm/s): ");
andjoh 0:fc81857d8067 160 swo.printf("%f ", V);
andjoh 0:fc81857d8067 161 swo.printf(" Counted encoder pulses: ");
andjoh 0:fc81857d8067 162 uint32_t countedPulses = pulseCountBetweenSensors[2] - pulseCountBetweenSensors[0];
andjoh 0:fc81857d8067 163 swo.printf("%d ", countedPulses);
andjoh 0:fc81857d8067 164 swo.printf(" ->Encoder speed (mm/s): ");
andjoh 0:fc81857d8067 165 float countedSpeed = 1000 * countedPulses / time;
andjoh 0:fc81857d8067 166 swo.printf("%f ", countedSpeed);
andjoh 0:fc81857d8067 167 swo.printf(" ->Speed difference (percentage): ");
andjoh 0:fc81857d8067 168 double difference = (100 * V / countedSpeed) - 100;
andjoh 0:fc81857d8067 169 swo.printf("%f ", difference);
andjoh 0:fc81857d8067 170
andjoh 0:fc81857d8067 171 swo.printf("\n");
andjoh 0:fc81857d8067 172
andjoh 0:fc81857d8067 173 time = (sensorNoteRunningTimes[4] - sensorNoteRunningTimes[2]) / 1000; //ms
andjoh 0:fc81857d8067 174 V = 1000 * BtoC_dist / time; //(mm/s)
andjoh 0:fc81857d8067 175
andjoh 0:fc81857d8067 176 swo.printf("Speed between second and third sensor (mm/s): ");
andjoh 0:fc81857d8067 177 swo.printf("%f ", V);
andjoh 0:fc81857d8067 178 swo.printf(" Counted encoder pulses: ");
andjoh 0:fc81857d8067 179 countedPulses = pulseCountBetweenSensors[4] - pulseCountBetweenSensors[2];
andjoh 0:fc81857d8067 180 swo.printf("%d ", countedPulses);
andjoh 0:fc81857d8067 181 swo.printf(" ->Encoder speed (mm/s): ");
andjoh 0:fc81857d8067 182 countedSpeed = 1000 * countedPulses / time;
andjoh 0:fc81857d8067 183 swo.printf("%f ", countedSpeed);
andjoh 0:fc81857d8067 184 swo.printf(" ->Speed difference (percentage): ");
andjoh 0:fc81857d8067 185 difference = (100 * V / countedSpeed) - 100;
andjoh 0:fc81857d8067 186 swo.printf("%f ", difference);
andjoh 0:fc81857d8067 187
andjoh 0:fc81857d8067 188 swo.printf("\n");
andjoh 0:fc81857d8067 189
andjoh 0:fc81857d8067 190 time = (sensorNoteRunningTimes[6] - sensorNoteRunningTimes[4]) / 1000; //ms
andjoh 0:fc81857d8067 191 V = 1000 * CtoD_dist / time; //(mm/s)
andjoh 0:fc81857d8067 192
andjoh 0:fc81857d8067 193 swo.printf("Speed between third and fourth sensor (mm/s): ");
andjoh 0:fc81857d8067 194 swo.printf("%f ", V);
andjoh 0:fc81857d8067 195 swo.printf(" Counted encoder pulses: ");
andjoh 0:fc81857d8067 196 countedPulses = pulseCountBetweenSensors[6] - pulseCountBetweenSensors[4];
andjoh 0:fc81857d8067 197 swo.printf("%d ", countedPulses);
andjoh 0:fc81857d8067 198 swo.printf(" ->Encoder speed (mm/s): ");
andjoh 0:fc81857d8067 199 countedSpeed = 1000 * countedPulses / time;
andjoh 0:fc81857d8067 200 swo.printf("%f ", countedSpeed);
andjoh 0:fc81857d8067 201 swo.printf(" ->Speed difference (percentage): ");
andjoh 0:fc81857d8067 202 difference = (100 * V / countedSpeed) - 100;
andjoh 0:fc81857d8067 203 swo.printf("%f ", difference);
andjoh 0:fc81857d8067 204
andjoh 0:fc81857d8067 205 swo.printf("\n");
andjoh 0:fc81857d8067 206
andjoh 0:fc81857d8067 207 time = (sensorNoteRunningTimes[8] - sensorNoteRunningTimes[6]) / 1000; //ms
andjoh 0:fc81857d8067 208 V = 1000 * DtoE_dist / time; //(mm/s)
andjoh 0:fc81857d8067 209
andjoh 0:fc81857d8067 210 swo.printf("Speed between fourth and fifth sensor (mm/s): ");
andjoh 0:fc81857d8067 211 swo.printf("%f ", V);
andjoh 0:fc81857d8067 212 swo.printf(" Counted encoder pulses: ");
andjoh 0:fc81857d8067 213 countedPulses = pulseCountBetweenSensors[8] - pulseCountBetweenSensors[6];
andjoh 0:fc81857d8067 214 swo.printf("%d ", countedPulses);
andjoh 0:fc81857d8067 215 swo.printf(" ->Encoder speed (mm/s): ");
andjoh 0:fc81857d8067 216 countedSpeed = 1000 * countedPulses / time;
andjoh 0:fc81857d8067 217 swo.printf("%f ", countedSpeed);
andjoh 0:fc81857d8067 218 swo.printf(" ->Speed difference (percentage): ");
andjoh 0:fc81857d8067 219 difference = (100 * V / countedSpeed) - 100;
andjoh 0:fc81857d8067 220 swo.printf("%f ", difference);
andjoh 0:fc81857d8067 221
andjoh 0:fc81857d8067 222 swo.printf("\n");
andjoh 0:fc81857d8067 223 swo.printf("\n");
andjoh 0:fc81857d8067 224
andjoh 0:fc81857d8067 225 time = (sensorNoteRunningTimes[8] - sensorNoteRunningTimes[0]) / 1000; //ms
andjoh 0:fc81857d8067 226 V = 1000 * Tot_dist / time; //(mm/s)
andjoh 0:fc81857d8067 227
andjoh 0:fc81857d8067 228 swo.printf("Total mean speed between first and last sensor (mm/s): ");
andjoh 0:fc81857d8067 229 swo.printf("%f ", V);
andjoh 0:fc81857d8067 230 swo.printf(" Total counted encoder pulses: ");
andjoh 0:fc81857d8067 231 countedPulses = pulseCountBetweenSensors[8] - pulseCountBetweenSensors[0];
andjoh 0:fc81857d8067 232 swo.printf("%d ", countedPulses);
andjoh 0:fc81857d8067 233 swo.printf(" ->Encoder speed (mm/s): ");
andjoh 0:fc81857d8067 234 countedSpeed = 1000 * countedPulses / time;
andjoh 0:fc81857d8067 235 swo.printf("%f ", countedSpeed);
andjoh 0:fc81857d8067 236 swo.printf(" ->Total speed difference (percentage): ");
andjoh 0:fc81857d8067 237 difference = (100 * V / countedSpeed) - 100;
andjoh 0:fc81857d8067 238 swo.printf("%f ", difference);
andjoh 0:fc81857d8067 239
andjoh 0:fc81857d8067 240 swo.printf("\n");
andjoh 0:fc81857d8067 241 swo.printf("\n");
andjoh 0:fc81857d8067 242
andjoh 0:fc81857d8067 243 swo.printf("Maximum time between pulses (us): %d\r\n", maxTimeBetweenPulses);
andjoh 0:fc81857d8067 244 swo.printf("Minimum time between pulses (us): %d\r\n", minTimeBetweenPulses);
andjoh 0:fc81857d8067 245
andjoh 0:fc81857d8067 246 swo.printf("\n");
andjoh 0:fc81857d8067 247 swo.printf("Filtered speed value to PID (mm/s): ");
andjoh 0:fc81857d8067 248 swo.printf("%f ", currentSpeed);
andjoh 0:fc81857d8067 249
andjoh 0:fc81857d8067 250
andjoh 0:fc81857d8067 251 swo.printf("\n");
andjoh 0:fc81857d8067 252 swo.printf("\n");
andjoh 0:fc81857d8067 253
andjoh 0:fc81857d8067 254
andjoh 0:fc81857d8067 255 reportNoteThroughSensorLengths();
andjoh 0:fc81857d8067 256
andjoh 0:fc81857d8067 257 maxTimeBetweenPulses = 0;
andjoh 0:fc81857d8067 258 minTimeBetweenPulses = 10000000;
andjoh 0:fc81857d8067 259 pulses = 0;
andjoh 0:fc81857d8067 260
andjoh 0:fc81857d8067 261 reportTimer = t.read_ms();
andjoh 0:fc81857d8067 262 }
andjoh 0:fc81857d8067 263
andjoh 0:fc81857d8067 264
andjoh 0:fc81857d8067 265
andjoh 0:fc81857d8067 266 void encTick(void)
andjoh 0:fc81857d8067 267 {
andjoh 0:fc81857d8067 268 long now = t.read_us();
andjoh 0:fc81857d8067 269 timeBetweenPulses = now - lastPulseTime;
andjoh 0:fc81857d8067 270
andjoh 0:fc81857d8067 271 if(noteInSystem) {
andjoh 0:fc81857d8067 272 if(timeBetweenPulses > maxTimeBetweenPulses) maxTimeBetweenPulses = timeBetweenPulses; //storing maximum time between pulses
andjoh 0:fc81857d8067 273 if(timeBetweenPulses < minTimeBetweenPulses) minTimeBetweenPulses = timeBetweenPulses; //storing minimum time between pulses
andjoh 0:fc81857d8067 274 }
andjoh 0:fc81857d8067 275
andjoh 0:fc81857d8067 276 lastPulseTime = now;
andjoh 0:fc81857d8067 277
andjoh 0:fc81857d8067 278 pulses++;
andjoh 0:fc81857d8067 279
andjoh 0:fc81857d8067 280 //filtered 10%
andjoh 0:fc81857d8067 281 currentSpeed = 0.05 * (1000000/timeBetweenPulses) + 0.95 * currentSpeed; //mm/s
andjoh 0:fc81857d8067 282 //currentSpeed = 1000000/timeBetweenPulses; //mm/s
andjoh 0:fc81857d8067 283 }
andjoh 0:fc81857d8067 284
andjoh 0:fc81857d8067 285 void checkOpticalSensors(void)
andjoh 0:fc81857d8067 286 {
andjoh 0:fc81857d8067 287
andjoh 0:fc81857d8067 288 for(int i=0; i<=5; i++) {
andjoh 0:fc81857d8067 289 opticalSensors[i]=0;
andjoh 0:fc81857d8067 290 }
andjoh 0:fc81857d8067 291
andjoh 0:fc81857d8067 292 if(SCANNER_INPUT) {
andjoh 0:fc81857d8067 293 opticalSensors[0]=1;
andjoh 0:fc81857d8067 294 if(lastValueOpticalSensors[0] == 0) {
andjoh 0:fc81857d8067 295 sensorNoteRunningTimes[0] = t.read_us(); //leading edge of note detected on first optical sensor - storing time
andjoh 0:fc81857d8067 296 noteInSystem = 1; //note entered the system
andjoh 0:fc81857d8067 297 pulseCountBetweenSensors[0] = pulses;
andjoh 0:fc81857d8067 298 }
andjoh 0:fc81857d8067 299 } else {
andjoh 0:fc81857d8067 300 if(lastValueOpticalSensors[0] == 1) {
andjoh 0:fc81857d8067 301 sensorNoteRunningTimes[1] = t.read_us(); //leaving edge of note detected on first optical sensor - storing time
andjoh 0:fc81857d8067 302 pulseCountBetweenSensors[1] = pulses;
andjoh 0:fc81857d8067 303 }
andjoh 0:fc81857d8067 304 }
andjoh 0:fc81857d8067 305
andjoh 0:fc81857d8067 306 if(SCANNER_OUTPUT) {
andjoh 0:fc81857d8067 307 opticalSensors[1]=1;
andjoh 0:fc81857d8067 308 if(lastValueOpticalSensors[1] == 0) {
andjoh 0:fc81857d8067 309 sensorNoteRunningTimes[2] = t.read_us(); //note detected on second optical sensor - storing time
andjoh 0:fc81857d8067 310 pulseCountBetweenSensors[2] = pulses;
andjoh 0:fc81857d8067 311 }
andjoh 0:fc81857d8067 312 } else {
andjoh 0:fc81857d8067 313 if(lastValueOpticalSensors[1] == 1) {
andjoh 0:fc81857d8067 314 sensorNoteRunningTimes[3] = t.read_us(); //leaving edge of note detected on first optical sensor - storing time
andjoh 0:fc81857d8067 315 pulseCountBetweenSensors[3] = pulses;
andjoh 0:fc81857d8067 316 }
andjoh 0:fc81857d8067 317 }
andjoh 0:fc81857d8067 318
andjoh 0:fc81857d8067 319 if(SWITCH_INPUT) {
andjoh 0:fc81857d8067 320 opticalSensors[2]=1;
andjoh 0:fc81857d8067 321 if(lastValueOpticalSensors[2] == 0) {
andjoh 0:fc81857d8067 322 sensorNoteRunningTimes[4] = t.read_us(); //note detected on second optical sensor - storing time
andjoh 0:fc81857d8067 323 pulseCountBetweenSensors[4] = pulses;
andjoh 0:fc81857d8067 324 }
andjoh 0:fc81857d8067 325 } else {
andjoh 0:fc81857d8067 326 if(lastValueOpticalSensors[2] == 1) {
andjoh 0:fc81857d8067 327 sensorNoteRunningTimes[5] = t.read_us(); //leaving edge of note detected on first optical sensor - storing time
andjoh 0:fc81857d8067 328 pulseCountBetweenSensors[5] = pulses;
andjoh 0:fc81857d8067 329 }
andjoh 0:fc81857d8067 330 }
andjoh 0:fc81857d8067 331
andjoh 0:fc81857d8067 332 if(REJECT_INPUT) {
andjoh 0:fc81857d8067 333 opticalSensors[3]=1;
andjoh 0:fc81857d8067 334 if(lastValueOpticalSensors[3] == 0) {
andjoh 0:fc81857d8067 335 sensorNoteRunningTimes[6] = t.read_us(); //note detected on second optical sensor - storing time
andjoh 0:fc81857d8067 336 pulseCountBetweenSensors[6] = pulses;
andjoh 0:fc81857d8067 337 }
andjoh 0:fc81857d8067 338 } else {
andjoh 0:fc81857d8067 339 if(lastValueOpticalSensors[3] == 1) {
andjoh 0:fc81857d8067 340 sensorNoteRunningTimes[7] = t.read_us(); //leaving edge of note detected on first optical sensor - storing time
andjoh 0:fc81857d8067 341 pulseCountBetweenSensors[7] = pulses;
andjoh 0:fc81857d8067 342 }
andjoh 0:fc81857d8067 343 }
andjoh 0:fc81857d8067 344
andjoh 0:fc81857d8067 345 if(REJECT_OUTPUT) {
andjoh 0:fc81857d8067 346 opticalSensors[4]=1;
andjoh 0:fc81857d8067 347 if(lastValueOpticalSensors[4] == 0) {
andjoh 0:fc81857d8067 348 sensorNoteRunningTimes[8] = t.read_us(); //note detected on second optical sensor - storing time
andjoh 0:fc81857d8067 349 pulseCountBetweenSensors[8] = pulses;
andjoh 0:fc81857d8067 350 }
andjoh 0:fc81857d8067 351 } else {
andjoh 0:fc81857d8067 352 if(lastValueOpticalSensors[4] == 1) {
andjoh 0:fc81857d8067 353 sensorNoteRunningTimes[9] = t.read_us(); //leaving edge of note detected on first optical sensor - storing time
andjoh 0:fc81857d8067 354 pulseCountBetweenSensors[9] = pulses;
andjoh 0:fc81857d8067 355 noteInSystem = 0; //note left the system
andjoh 0:fc81857d8067 356 report();
andjoh 0:fc81857d8067 357 }
andjoh 0:fc81857d8067 358 }
andjoh 0:fc81857d8067 359
andjoh 0:fc81857d8067 360 if(STORAGE_INPUT) opticalSensors[5]=1;
andjoh 0:fc81857d8067 361
andjoh 0:fc81857d8067 362 //storing sensor state in previous value array
andjoh 0:fc81857d8067 363 for(int i=0; i<=5; i++) {
andjoh 0:fc81857d8067 364 lastValueOpticalSensors[i] = opticalSensors[i];
andjoh 0:fc81857d8067 365 }
andjoh 0:fc81857d8067 366
andjoh 0:fc81857d8067 367 }
andjoh 0:fc81857d8067 368
andjoh 0:fc81857d8067 369 void tunePID()
andjoh 0:fc81857d8067 370 {
andjoh 0:fc81857d8067 371
andjoh 0:fc81857d8067 372 swo.printf("setting 60percent output %\n");
andjoh 0:fc81857d8067 373 MAIN_PWM.write(0.6f);
andjoh 0:fc81857d8067 374 swo.printf("waiting for speedup... %\n");
andjoh 0:fc81857d8067 375 wait(2); //waiting 2 sec
andjoh 0:fc81857d8067 376 swo.printf("Current speed (mm/s): %f\n", currentSpeed);
andjoh 0:fc81857d8067 377 double beforeSpeed = currentSpeed;
andjoh 0:fc81857d8067 378 swo.printf("setting 70percent output %\n");
andjoh 0:fc81857d8067 379 MAIN_PWM.write(0.7f); //10% more output
andjoh 0:fc81857d8067 380 swo.printf("waiting for speedup... %\n");
andjoh 0:fc81857d8067 381 wait(2); //waiting 2 sec
andjoh 0:fc81857d8067 382 swo.printf("Current speed (mm/s): %f\n", currentSpeed);
andjoh 0:fc81857d8067 383 double deltaSpeed = currentSpeed - beforeSpeed;
andjoh 0:fc81857d8067 384 swo.printf("delta Speed (mm/s): %f\n", deltaSpeed);
andjoh 0:fc81857d8067 385 double K = deltaSpeed / 10;
andjoh 0:fc81857d8067 386 swo.printf("PID K-factor (deltaSpeed/10): %f\n", K);
andjoh 0:fc81857d8067 387
andjoh 0:fc81857d8067 388 controller.setTunings(K/2, 0, 0); //K/2?
andjoh 0:fc81857d8067 389
andjoh 0:fc81857d8067 390 controller.setInputLimits(0.0, 600);
andjoh 0:fc81857d8067 391
andjoh 0:fc81857d8067 392 //Pwm output from 0.0 to 1.0
andjoh 0:fc81857d8067 393 controller.setOutputLimits(0.0, 1.0);
andjoh 0:fc81857d8067 394
andjoh 0:fc81857d8067 395 controller.setMode(AUTO_MODE);
andjoh 0:fc81857d8067 396
andjoh 0:fc81857d8067 397 controller.setSetPoint(600);
andjoh 0:fc81857d8067 398
andjoh 0:fc81857d8067 399 swo.printf("\n");
andjoh 0:fc81857d8067 400 swo.printf("\n");
andjoh 0:fc81857d8067 401 swo.printf("\n");
andjoh 0:fc81857d8067 402
andjoh 0:fc81857d8067 403 }
andjoh 0:fc81857d8067 404
andjoh 0:fc81857d8067 405
andjoh 0:fc81857d8067 406 int main()
andjoh 0:fc81857d8067 407 {
andjoh 0:fc81857d8067 408 LED_ENABLE = 1; //turning leds on
andjoh 0:fc81857d8067 409 LED_GREEN = 0;
andjoh 0:fc81857d8067 410 LED_RED = 0;
andjoh 0:fc81857d8067 411
andjoh 0:fc81857d8067 412 //Setting motors off
andjoh 0:fc81857d8067 413 MAIN_PWM.write(0.00f);
andjoh 0:fc81857d8067 414 SWITCH_PWM.write(0.00f);
andjoh 0:fc81857d8067 415 ROUTER_PWM.write(0.00f);
andjoh 0:fc81857d8067 416 STORAGE_PWM.write(0.00f);
andjoh 0:fc81857d8067 417 MAIN_PWM.period(0.00005); //Set motor PWM periods to 20KHz.
andjoh 0:fc81857d8067 418 SWITCH_PWM.period(0.00005); //Set motor PWM periods to 20KHz.
andjoh 0:fc81857d8067 419 ROUTER_PWM.period(0.00005); //Set motor PWM periods to 20KHz.
andjoh 0:fc81857d8067 420 STORAGE_PWM.period(0.00005); //Set motor PWM periods to 20KHz.
andjoh 0:fc81857d8067 421
andjoh 0:fc81857d8067 422
andjoh 0:fc81857d8067 423 swo.printf("STARTED %\n");
andjoh 0:fc81857d8067 424 swo.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock); //SYTEM CLOCK 72MHz
andjoh 0:fc81857d8067 425 MAIN_ENC.rise(&encTick); // attach the address of the encTick function to the rising edge
andjoh 0:fc81857d8067 426
andjoh 0:fc81857d8067 427 t.start();
andjoh 0:fc81857d8067 428
andjoh 0:fc81857d8067 429 //MAIN_PWM.write(0.7f); //40% works 0.4f
andjoh 0:fc81857d8067 430
andjoh 0:fc81857d8067 431 tunePID();
andjoh 0:fc81857d8067 432
andjoh 0:fc81857d8067 433
andjoh 0:fc81857d8067 434 while(1) {
andjoh 0:fc81857d8067 435
andjoh 0:fc81857d8067 436
andjoh 0:fc81857d8067 437 checkOpticalSensors(); //updating opticalSensors array
andjoh 0:fc81857d8067 438
andjoh 0:fc81857d8067 439 //if(t.read_ms() - reportTimer > 1000 && noteInSystem == 0) report(); //reporting every second
andjoh 0:fc81857d8067 440
andjoh 0:fc81857d8067 441 if(t.read_ms() - lastControl > RATE) {
andjoh 0:fc81857d8067 442 //Update the process variable.
andjoh 0:fc81857d8067 443 controller.setProcessValue(currentSpeed);
andjoh 0:fc81857d8067 444 //Set the new output.
andjoh 0:fc81857d8067 445 double output = controller.compute();
andjoh 0:fc81857d8067 446 MAIN_PWM.write(output);
andjoh 0:fc81857d8067 447 //swo.printf("sent to controller: %f\n", output);
andjoh 0:fc81857d8067 448 lastControl = t.read_ms();
andjoh 0:fc81857d8067 449 }
andjoh 0:fc81857d8067 450
andjoh 0:fc81857d8067 451 }
andjoh 0:fc81857d8067 452
andjoh 0:fc81857d8067 453
andjoh 0:fc81857d8067 454 }
andjoh 0:fc81857d8067 455
andjoh 0:fc81857d8067 456