Pawel Stepien
CHR-6dm trial
main.cpp@0:e35c4c5a52d6, 2011-04-15 (annotated)
- Committer:
- kinemax
- Date:
- Fri Apr 15 08:04:23 2011 +0000
- Revision:
- 0:e35c4c5a52d6
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| kinemax | 0:e35c4c5a52d6 | 1 | // Example PwmIn class to detect PWM inputs, sford |
| kinemax | 0:e35c4c5a52d6 | 2 | // - Note: uses InterruptIn, so not available on p19/p20 |
| kinemax | 0:e35c4c5a52d6 | 3 | |
| kinemax | 0:e35c4c5a52d6 | 4 | #include "mbed.h" |
| kinemax | 0:e35c4c5a52d6 | 5 | //#include "PwmIn.h" |
| kinemax | 0:e35c4c5a52d6 | 6 | |
| kinemax | 0:e35c4c5a52d6 | 7 | #define BAUD 115200 |
| kinemax | 0:e35c4c5a52d6 | 8 | #define TX_PIN p9 |
| kinemax | 0:e35c4c5a52d6 | 9 | #define RX_PIN p10 |
| kinemax | 0:e35c4c5a52d6 | 10 | |
| kinemax | 0:e35c4c5a52d6 | 11 | #include "MODSERIAL.h" |
| kinemax | 0:e35c4c5a52d6 | 12 | |
| kinemax | 0:e35c4c5a52d6 | 13 | DigitalOut led1(LED1); |
| kinemax | 0:e35c4c5a52d6 | 14 | DigitalOut led2(LED2); |
| kinemax | 0:e35c4c5a52d6 | 15 | DigitalOut led3(LED3); |
| kinemax | 0:e35c4c5a52d6 | 16 | DigitalOut led4(LED4); |
| kinemax | 0:e35c4c5a52d6 | 17 | |
| kinemax | 0:e35c4c5a52d6 | 18 | MODSERIAL pc(USBTX, USBRX); |
| kinemax | 0:e35c4c5a52d6 | 19 | MODSERIAL uart(TX_PIN, RX_PIN); |
| kinemax | 0:e35c4c5a52d6 | 20 | |
| kinemax | 0:e35c4c5a52d6 | 21 | bool newpacket = false; |
| kinemax | 0:e35c4c5a52d6 | 22 | |
| kinemax | 0:e35c4c5a52d6 | 23 | //PwmIn a(p21), b(p22), c(p23), d(p24), e(p25); |
| kinemax | 0:e35c4c5a52d6 | 24 | |
| kinemax | 0:e35c4c5a52d6 | 25 | int yaw=0,pitch=0,roll=0, Dyaw=0,Dpitch=0,Droll=0,chk=0; |
| kinemax | 0:e35c4c5a52d6 | 26 | |
| kinemax | 0:e35c4c5a52d6 | 27 | int main() |
| kinemax | 0:e35c4c5a52d6 | 28 | { char ileB=0,kan1=0,kan2=0; |
| kinemax | 0:e35c4c5a52d6 | 29 | char zna='0', znak, pakiet[255]="aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa"; |
| kinemax | 0:e35c4c5a52d6 | 30 | int licz=0; |
| kinemax | 0:e35c4c5a52d6 | 31 | pc.baud(BAUD); |
| kinemax | 0:e35c4c5a52d6 | 32 | uart.baud(BAUD); |
| kinemax | 0:e35c4c5a52d6 | 33 | // uart.attach(&rxCallback, MODSERIAL::RxIrq); |
| kinemax | 0:e35c4c5a52d6 | 34 | pc.printf("Start\n\r"); |
| kinemax | 0:e35c4c5a52d6 | 35 | |
| kinemax | 0:e35c4c5a52d6 | 36 | while(1) |
| kinemax | 0:e35c4c5a52d6 | 37 | { |
| kinemax | 0:e35c4c5a52d6 | 38 | if( uart.readable() ); |
| kinemax | 0:e35c4c5a52d6 | 39 | led1 = 1; // Show start of sending with LED1. |
| kinemax | 0:e35c4c5a52d6 | 40 | while( (znak=uart.getc())!=115); |
| kinemax | 0:e35c4c5a52d6 | 41 | { led2 =1; |
| kinemax | 0:e35c4c5a52d6 | 42 | if(uart.getc()==110) |
| kinemax | 0:e35c4c5a52d6 | 43 | { |
| kinemax | 0:e35c4c5a52d6 | 44 | if(uart.getc()==112) |
| kinemax | 0:e35c4c5a52d6 | 45 | { |
| kinemax | 0:e35c4c5a52d6 | 46 | if(uart.getc()==0xB7) |
| kinemax | 0:e35c4c5a52d6 | 47 | { led3=1; |
| kinemax | 0:e35c4c5a52d6 | 48 | ileB=uart.getc(); |
| kinemax | 0:e35c4c5a52d6 | 49 | for(int i=0; i<ileB+2; i++) pakiet[i]=uart.getc(); |
| kinemax | 0:e35c4c5a52d6 | 50 | /* |
| kinemax | 0:e35c4c5a52d6 | 51 | kan1=uart.getc(); kan2= uart.getc(); |
| kinemax | 0:e35c4c5a52d6 | 52 | yaw = 256 * (int)uart.getc() + uart.getc(); |
| kinemax | 0:e35c4c5a52d6 | 53 | pitch = 256 * (int)uart.getc() + uart.getc(); |
| kinemax | 0:e35c4c5a52d6 | 54 | roll = 256 * (int)uart.getc() + uart.getc(); |
| kinemax | 0:e35c4c5a52d6 | 55 | chk = 256 * (int)uart.getc() + uart.getc(); |
| kinemax | 0:e35c4c5a52d6 | 56 | */ |
| kinemax | 0:e35c4c5a52d6 | 57 | led4=1; led1=0; led2=0; led3=0; |
| kinemax | 0:e35c4c5a52d6 | 58 | newpacket=1; |
| kinemax | 0:e35c4c5a52d6 | 59 | //pc.txBufferFlush(); |
| kinemax | 0:e35c4c5a52d6 | 60 | pc.printf("\n\r N = %2d, NoBytes: ", ileB); |
| kinemax | 0:e35c4c5a52d6 | 61 | for(int i=0; i<ileB+2; i++) |
| kinemax | 0:e35c4c5a52d6 | 62 | pc.printf(" %3d ",pakiet[i]); |
| kinemax | 0:e35c4c5a52d6 | 63 | wait(1); |
| kinemax | 0:e35c4c5a52d6 | 64 | } |
| kinemax | 0:e35c4c5a52d6 | 65 | else pc.printf("No PT"); |
| kinemax | 0:e35c4c5a52d6 | 66 | } |
| kinemax | 0:e35c4c5a52d6 | 67 | else pc.printf("No P"); |
| kinemax | 0:e35c4c5a52d6 | 68 | } |
| kinemax | 0:e35c4c5a52d6 | 69 | else pc.printf("No N"); |
| kinemax | 0:e35c4c5a52d6 | 70 | } |
| kinemax | 0:e35c4c5a52d6 | 71 | |
| kinemax | 0:e35c4c5a52d6 | 72 | } |
| kinemax | 0:e35c4c5a52d6 | 73 | |
| kinemax | 0:e35c4c5a52d6 | 74 | //pc.printf("Kanaly= %5.3f %5.3f %5.3f %5.3f %5.3f\n\r", a.pulsewidth()*1000, a.pulsewidth()*1000, b.pulsewidth()*1000, c.pulsewidth()*1000, d.pulsewidth()*1000, e.pulsewidth()*1000); |
| kinemax | 0:e35c4c5a52d6 | 75 | //wait(1); |
| kinemax | 0:e35c4c5a52d6 | 76 | |
| kinemax | 0:e35c4c5a52d6 | 77 | } |
| kinemax | 0:e35c4c5a52d6 | 78 | |
| kinemax | 0:e35c4c5a52d6 | 79 | |
| kinemax | 0:e35c4c5a52d6 | 80 | // higher than "uart" baud rate below. |