Test the speed of sensor module PID controlled speed. When note is passed the optical sensors data is collected and shared via 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?
User | Revision | Line number | New 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 |