Torsten Wylegala
/
parallel_parking
foo
Fork of parallel_parking by
main.cpp@1:719ac5a42c19, 2016-02-08 (annotated)
- Committer:
- becheo
- Date:
- Mon Feb 08 11:13:44 2016 +0000
- Revision:
- 1:719ac5a42c19
- Parent:
- 0:c871d5355b99
- Child:
- 2:fae63e87900e
Testprogramm
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
becheo | 0:c871d5355b99 | 1 | #include <mbed.h> |
becheo | 1:719ac5a42c19 | 2 | #include "rtos.h" |
becheo | 0:c871d5355b99 | 3 | #include "Periphery/SupportSystem.h" |
becheo | 0:c871d5355b99 | 4 | #include "Misc/SystemTimer.h" |
becheo | 0:c871d5355b99 | 5 | |
becheo | 0:c871d5355b99 | 6 | Serial serialXbee(p28,p27), serialMinnow(p13, p14); |
becheo | 0:c871d5355b99 | 7 | PwmOut drivePWM(p22), steerPWM(p21); |
becheo | 1:719ac5a42c19 | 8 | |
becheo | 0:c871d5355b99 | 9 | |
becheo | 0:c871d5355b99 | 10 | DigitalOut serialLED(LED1), ledTwo(LED2), ledThree(LED3), heartbeatLED(LED4); |
becheo | 0:c871d5355b99 | 11 | |
becheo | 0:c871d5355b99 | 12 | DigitalIn buttonOne(p25), buttonTwo(p26), buttonThree(p29), buttonFour(p30); |
becheo | 0:c871d5355b99 | 13 | DigitalIn redlight(p16); |
becheo | 0:c871d5355b99 | 14 | |
becheo | 1:719ac5a42c19 | 15 | |
becheo | 0:c871d5355b99 | 16 | IMU_RegisterDataBuffer_t *IMU_registerDataBuffer; |
becheo | 1:719ac5a42c19 | 17 | Timer timer; |
becheo | 0:c871d5355b99 | 18 | |
becheo | 0:c871d5355b99 | 19 | void hearbeat_thread(void const *args); |
becheo | 0:c871d5355b99 | 20 | |
becheo | 0:c871d5355b99 | 21 | int main() { |
becheo | 1:719ac5a42c19 | 22 | serialMinnow.baud(115200); |
becheo | 1:719ac5a42c19 | 23 | I2C *i2c = new I2C(p9, p10); |
becheo | 1:719ac5a42c19 | 24 | |
becheo | 0:c871d5355b99 | 25 | drivePWM.period_ms(20); |
becheo | 0:c871d5355b99 | 26 | steerPWM.period_ms(20); |
becheo | 0:c871d5355b99 | 27 | |
becheo | 0:c871d5355b99 | 28 | SystemTimer *millis = new SystemTimer(); |
becheo | 1:719ac5a42c19 | 29 | SupportSystem *supportSystem = new SupportSystem(0x80, i2c); |
becheo | 1:719ac5a42c19 | 30 | |
becheo | 0:c871d5355b99 | 31 | Thread hearbeatThread(hearbeat_thread); |
becheo | 1:719ac5a42c19 | 32 | |
becheo | 1:719ac5a42c19 | 33 | steerPWM.pulsewidth_us(1500); |
becheo | 1:719ac5a42c19 | 34 | float strecke_anfang, strecke_ende, size_parking_space, zeit_anfang, zeit_ende, zeit_parking_space; |
becheo | 1:719ac5a42c19 | 35 | timer.start(); |
becheo | 0:c871d5355b99 | 36 | |
becheo | 1:719ac5a42c19 | 37 | // Konfiguration AMF-IMU |
becheo | 1:719ac5a42c19 | 38 | // [0]: Conversation Factor |
becheo | 1:719ac5a42c19 | 39 | // [1]: Sensor Position X |
becheo | 1:719ac5a42c19 | 40 | // [2]: Sensor Position Y |
becheo | 1:719ac5a42c19 | 41 | // [3]: Sensor Position Angle |
becheo | 1:719ac5a42c19 | 42 | //float configData[4] = {0.002751114f, 167.0f, 0.0f, 271.5f}; |
becheo | 1:719ac5a42c19 | 43 | //supportSystem->writeImuConfig(configData); |
becheo | 1:719ac5a42c19 | 44 | |
becheo | 1:719ac5a42c19 | 45 | /* while(true){ |
becheo | 1:719ac5a42c19 | 46 | |
becheo | 1:719ac5a42c19 | 47 | ledTwo = !ledTwo; |
becheo | 1:719ac5a42c19 | 48 | IMU_registerDataBuffer = supportSystem->getImuRegisterDataBuffer(); |
becheo | 1:719ac5a42c19 | 49 | |
becheo | 1:719ac5a42c19 | 50 | |
becheo | 1:719ac5a42c19 | 51 | serialMinnow.printf("X %f\n\r", IMU_registerDataBuffer->distanceXRegister); |
becheo | 1:719ac5a42c19 | 52 | serialMinnow.printf("S %f\n\r", IMU_registerDataBuffer->squal); |
becheo | 1:719ac5a42c19 | 53 | serialMinnow.printf("Uptime %d\n\r", supportSystem->readMaintenanceUptimeMillis()); |
becheo | 1:719ac5a42c19 | 54 | serialMinnow.printf("Error: %d\n\r", supportSystem->getTransmissionErrorCount()); |
becheo | 1:719ac5a42c19 | 55 | |
becheo | 1:719ac5a42c19 | 56 | wait(0.100); |
becheo | 1:719ac5a42c19 | 57 | } |
becheo | 1:719ac5a42c19 | 58 | */ |
becheo | 0:c871d5355b99 | 59 | |
becheo | 0:c871d5355b99 | 60 | while(true){ |
becheo | 1:719ac5a42c19 | 61 | ledTwo = !ledTwo; |
becheo | 1:719ac5a42c19 | 62 | IMU_registerDataBuffer = supportSystem->getImuRegisterDataBuffer(); |
becheo | 1:719ac5a42c19 | 63 | |
becheo | 0:c871d5355b99 | 64 | if(buttonFour == 1){ |
becheo | 1:719ac5a42c19 | 65 | |
becheo | 1:719ac5a42c19 | 66 | drivePWM.pulsewidth_us(1700); |
becheo | 1:719ac5a42c19 | 67 | while(true){ |
becheo | 1:719ac5a42c19 | 68 | if(redlight == 1){ |
becheo | 1:719ac5a42c19 | 69 | ledThree = 1; |
becheo | 1:719ac5a42c19 | 70 | |
becheo | 1:719ac5a42c19 | 71 | /*do{ |
becheo | 1:719ac5a42c19 | 72 | |
becheo | 1:719ac5a42c19 | 73 | }while(redlight == 1); |
becheo | 1:719ac5a42c19 | 74 | strecke_anfang = IMU_registerDataBuffer->distanceXRegister; |
becheo | 1:719ac5a42c19 | 75 | ledThree = 1; |
becheo | 1:719ac5a42c19 | 76 | do{ |
becheo | 1:719ac5a42c19 | 77 | |
becheo | 1:719ac5a42c19 | 78 | }while(redlight == 0);*/ |
becheo | 1:719ac5a42c19 | 79 | |
becheo | 1:719ac5a42c19 | 80 | /*strecke_anfang = IMU_registerDataBuffer->distanceXRegister; |
becheo | 1:719ac5a42c19 | 81 | wait_ms(2000); |
becheo | 1:719ac5a42c19 | 82 | strecke_ende = IMU_registerDataBuffer->distanceXRegister; |
becheo | 1:719ac5a42c19 | 83 | size_parking_space = strecke_ende - strecke_anfang; |
becheo | 1:719ac5a42c19 | 84 | serialMinnow.printf("%f, ", IMU_registerDataBuffer->distanceXRegister); |
becheo | 1:719ac5a42c19 | 85 | serialMinnow.printf("%f\r\n", size_parking_space);*/ |
becheo | 1:719ac5a42c19 | 86 | |
becheo | 1:719ac5a42c19 | 87 | do{ |
becheo | 1:719ac5a42c19 | 88 | |
becheo | 1:719ac5a42c19 | 89 | }while(redlight == 1); |
becheo | 1:719ac5a42c19 | 90 | zeit_anfang = timer.read_ms(); |
becheo | 1:719ac5a42c19 | 91 | do{ |
becheo | 1:719ac5a42c19 | 92 | |
becheo | 1:719ac5a42c19 | 93 | }while(redlight == 0); |
becheo | 1:719ac5a42c19 | 94 | zeit_ende = timer.read_ms(); |
becheo | 1:719ac5a42c19 | 95 | zeit_parking_space = zeit_ende - zeit_anfang; |
becheo | 1:719ac5a42c19 | 96 | serialMinnow.printf("Differenz: %f\r\n", zeit_parking_space); |
becheo | 1:719ac5a42c19 | 97 | } |
becheo | 1:719ac5a42c19 | 98 | if(zeit_parking_space < 1350 && zeit_parking_space > 1100){ |
becheo | 1:719ac5a42c19 | 99 | |
becheo | 1:719ac5a42c19 | 100 | drivePWM.pulsewidth_us(1700); //Stück nach vorne fahren |
becheo | 1:719ac5a42c19 | 101 | wait_ms(200); |
becheo | 1:719ac5a42c19 | 102 | drivePWM.pulsewidth_us(1500); //Stehen |
becheo | 1:719ac5a42c19 | 103 | wait_ms(200); |
becheo | 1:719ac5a42c19 | 104 | drivePWM.pulsewidth_us(1325); //1. Einschlag |
becheo | 1:719ac5a42c19 | 105 | steerPWM.pulsewidth_us(2000); |
becheo | 1:719ac5a42c19 | 106 | wait_ms(970); |
becheo | 1:719ac5a42c19 | 107 | steerPWM.pulsewidth_us(1000); //2. Einschlag |
becheo | 1:719ac5a42c19 | 108 | wait_ms(950); |
becheo | 1:719ac5a42c19 | 109 | drivePWM.pulsewidth_us(1700); //Korrektur nach vorne |
becheo | 1:719ac5a42c19 | 110 | steerPWM.pulsewidth_us(1700); |
becheo | 1:719ac5a42c19 | 111 | wait_ms(420); |
becheo | 1:719ac5a42c19 | 112 | drivePWM.pulsewidth_us(1500); //Ende |
becheo | 1:719ac5a42c19 | 113 | steerPWM.pulsewidth_us(1500); |
becheo | 1:719ac5a42c19 | 114 | break; |
becheo | 1:719ac5a42c19 | 115 | } |
becheo | 1:719ac5a42c19 | 116 | } |
becheo | 1:719ac5a42c19 | 117 | } |
becheo | 1:719ac5a42c19 | 118 | if(buttonOne == 1){ |
becheo | 1:719ac5a42c19 | 119 | ledTwo = 0; |
becheo | 1:719ac5a42c19 | 120 | ledThree = 0; |
becheo | 1:719ac5a42c19 | 121 | drivePWM.pulsewidth_us(1500); |
becheo | 1:719ac5a42c19 | 122 | } |
becheo | 1:719ac5a42c19 | 123 | wait_ms(100); |
becheo | 1:719ac5a42c19 | 124 | } |
becheo | 1:719ac5a42c19 | 125 | } |
becheo | 1:719ac5a42c19 | 126 | |
becheo | 1:719ac5a42c19 | 127 | |
becheo | 1:719ac5a42c19 | 128 | void hearbeat_thread(void const *args) { |
becheo | 1:719ac5a42c19 | 129 | while(true) { |
becheo | 1:719ac5a42c19 | 130 | heartbeatLED = !heartbeatLED; |
becheo | 1:719ac5a42c19 | 131 | Thread::wait(100); |
becheo | 1:719ac5a42c19 | 132 | } |
becheo | 1:719ac5a42c19 | 133 | } |
becheo | 1:719ac5a42c19 | 134 | |
becheo | 1:719ac5a42c19 | 135 | |
becheo | 1:719ac5a42c19 | 136 | |
becheo | 0:c871d5355b99 | 137 | //------- 1 ---------------- |
becheo | 0:c871d5355b99 | 138 | /* drivePWM.pulsewidth_us(1700); //Vorwärts |
becheo | 0:c871d5355b99 | 139 | wait_ms(2500); |
becheo | 0:c871d5355b99 | 140 | drivePWM.pulsewidth_us(1500); //Stehen |
becheo | 0:c871d5355b99 | 141 | wait_ms(200); |
becheo | 0:c871d5355b99 | 142 | drivePWM.pulsewidth_us(1325); //1. Einschlag |
becheo | 0:c871d5355b99 | 143 | steerPWM.pulsewidth_us(2000); |
becheo | 0:c871d5355b99 | 144 | wait_ms(970); |
becheo | 0:c871d5355b99 | 145 | steerPWM.pulsewidth_us(1000); //2. Einschlag |
becheo | 0:c871d5355b99 | 146 | wait_ms(950); |
becheo | 0:c871d5355b99 | 147 | drivePWM.pulsewidth_us(1700); //Korrektur nach vorne |
becheo | 0:c871d5355b99 | 148 | steerPWM.pulsewidth_us(1700); |
becheo | 0:c871d5355b99 | 149 | wait_ms(420); |
becheo | 0:c871d5355b99 | 150 | drivePWM.pulsewidth_us(1500); //Ende |
becheo | 0:c871d5355b99 | 151 | steerPWM.pulsewidth_us(1500); |
becheo | 1:719ac5a42c19 | 152 | ledTwo = 1; |
becheo | 0:c871d5355b99 | 153 | //------- 2 ---------------- |
becheo | 0:c871d5355b99 | 154 | drivePWM.pulsewidth_us(1800); //Vorwärts |
becheo | 0:c871d5355b99 | 155 | wait_ms(825); |
becheo | 0:c871d5355b99 | 156 | drivePWM.pulsewidth_us(1500); //Stehen |
becheo | 0:c871d5355b99 | 157 | wait_ms(200); |
becheo | 0:c871d5355b99 | 158 | drivePWM.pulsewidth_us(1300); //1. Einschlag |
becheo | 0:c871d5355b99 | 159 | steerPWM.pulsewidth_us(2000); |
becheo | 0:c871d5355b99 | 160 | wait_ms(680); |
becheo | 0:c871d5355b99 | 161 | steerPWM.pulsewidth_us(1000); //2. Einschlag |
becheo | 0:c871d5355b99 | 162 | wait_ms(630); |
becheo | 0:c871d5355b99 | 163 | drivePWM.pulsewidth_us(1700); //Korrektur nach vorne |
becheo | 0:c871d5355b99 | 164 | steerPWM.pulsewidth_us(1700); |
becheo | 0:c871d5355b99 | 165 | wait_ms(420); |
becheo | 0:c871d5355b99 | 166 | drivePWM.pulsewidth_us(1500); //Ende |
becheo | 0:c871d5355b99 | 167 | steerPWM.pulsewidth_us(1500); |
becheo | 1:719ac5a42c19 | 168 | */ |