Torsten Wylegala / Mbed 2 deprecated parallel_parking

foo

Dependencies:   mbed-rtos mbed

Fork of parallel_parking by Oliver Becher

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?

UserRevisionLine numberNew 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 */