Nu werkt het wel, opdracht 1 is af
Dependencies: MODSERIAL mbed QEI feed_forward
Fork of feed_forward by
main.cpp@2:a64e3c37b571, 2017-10-11 (annotated)
- Committer:
- DiondeGreef
- Date:
- Wed Oct 11 07:54:25 2017 +0000
- Revision:
- 2:a64e3c37b571
- Parent:
- 1:92a60278860a
Servomotoren compleet
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
RoyvZ | 0:331597250051 | 1 | #include "mbed.h" |
RoyvZ | 0:331597250051 | 2 | #include "MODSERIAL.h" |
RoyvZ | 0:331597250051 | 3 | #include "math.h" |
RoyvZ | 0:331597250051 | 4 | |
DiondeGreef | 2:a64e3c37b571 | 5 | DigitalOut Ledr(LED_RED); |
DiondeGreef | 2:a64e3c37b571 | 6 | DigitalOut Ledg(LED_GREEN); |
DiondeGreef | 2:a64e3c37b571 | 7 | DigitalOut Ledb(LED_BLUE); |
DiondeGreef | 2:a64e3c37b571 | 8 | PwmOut motorSpeed(D13); |
DiondeGreef | 2:a64e3c37b571 | 9 | PwmOut motorSpeed2(D12); |
DiondeGreef | 2:a64e3c37b571 | 10 | InterruptIn Button1(PTC6); |
DiondeGreef | 2:a64e3c37b571 | 11 | Ticker tick; |
RoyvZ | 0:331597250051 | 12 | |
RoyvZ | 0:331597250051 | 13 | |
DiondeGreef | 2:a64e3c37b571 | 14 | enum states{Close, Open}; |
DiondeGreef | 2:a64e3c37b571 | 15 | |
DiondeGreef | 2:a64e3c37b571 | 16 | states CurrentState = Open; |
DiondeGreef | 2:a64e3c37b571 | 17 | |
DiondeGreef | 2:a64e3c37b571 | 18 | int i = 0; |
DiondeGreef | 2:a64e3c37b571 | 19 | int j = 0; |
RoyvZ | 0:331597250051 | 20 | |
DiondeGreef | 2:a64e3c37b571 | 21 | void Change() |
RoyvZ | 0:331597250051 | 22 | { |
DiondeGreef | 2:a64e3c37b571 | 23 | if(CurrentState == Close){ |
DiondeGreef | 2:a64e3c37b571 | 24 | CurrentState = Open; |
DiondeGreef | 2:a64e3c37b571 | 25 | } |
DiondeGreef | 2:a64e3c37b571 | 26 | else{ |
DiondeGreef | 2:a64e3c37b571 | 27 | CurrentState = Close; |
DiondeGreef | 2:a64e3c37b571 | 28 | } |
RoyvZ | 0:331597250051 | 29 | } |
RoyvZ | 0:331597250051 | 30 | |
RoyvZ | 0:331597250051 | 31 | |
DiondeGreef | 2:a64e3c37b571 | 32 | |
DiondeGreef | 2:a64e3c37b571 | 33 | void ProcessStateMachine(void) |
RoyvZ | 0:331597250051 | 34 | { |
DiondeGreef | 2:a64e3c37b571 | 35 | switch (CurrentState) |
DiondeGreef | 2:a64e3c37b571 | 36 | { |
DiondeGreef | 2:a64e3c37b571 | 37 | case Close: |
DiondeGreef | 2:a64e3c37b571 | 38 | if(j <= 34){ |
DiondeGreef | 2:a64e3c37b571 | 39 | motorSpeed.write(0); |
DiondeGreef | 2:a64e3c37b571 | 40 | motorSpeed2.write(0); |
DiondeGreef | 2:a64e3c37b571 | 41 | j++; |
DiondeGreef | 2:a64e3c37b571 | 42 | } |
DiondeGreef | 2:a64e3c37b571 | 43 | else if (j == 35){ |
DiondeGreef | 2:a64e3c37b571 | 44 | motorSpeed.write(1); |
DiondeGreef | 2:a64e3c37b571 | 45 | motorSpeed2.write(0); |
DiondeGreef | 2:a64e3c37b571 | 46 | j++; |
DiondeGreef | 2:a64e3c37b571 | 47 | } |
DiondeGreef | 2:a64e3c37b571 | 48 | else if (j == 36){ |
DiondeGreef | 2:a64e3c37b571 | 49 | motorSpeed.write(1); |
DiondeGreef | 2:a64e3c37b571 | 50 | motorSpeed2.write(0); |
DiondeGreef | 2:a64e3c37b571 | 51 | j++; |
DiondeGreef | 2:a64e3c37b571 | 52 | } |
DiondeGreef | 2:a64e3c37b571 | 53 | else if (j == 37){ |
DiondeGreef | 2:a64e3c37b571 | 54 | motorSpeed.write(1); |
DiondeGreef | 2:a64e3c37b571 | 55 | motorSpeed2.write(0); |
DiondeGreef | 2:a64e3c37b571 | 56 | j++; |
DiondeGreef | 2:a64e3c37b571 | 57 | } |
DiondeGreef | 2:a64e3c37b571 | 58 | else if (j == 38){ |
DiondeGreef | 2:a64e3c37b571 | 59 | motorSpeed.write(1); |
DiondeGreef | 2:a64e3c37b571 | 60 | motorSpeed2.write(0); |
DiondeGreef | 2:a64e3c37b571 | 61 | j++; |
DiondeGreef | 2:a64e3c37b571 | 62 | } |
DiondeGreef | 2:a64e3c37b571 | 63 | else if (j == 39){ |
DiondeGreef | 2:a64e3c37b571 | 64 | motorSpeed.write(1); |
DiondeGreef | 2:a64e3c37b571 | 65 | motorSpeed2.write(1); |
DiondeGreef | 2:a64e3c37b571 | 66 | j = 0; |
DiondeGreef | 2:a64e3c37b571 | 67 | } |
DiondeGreef | 2:a64e3c37b571 | 68 | Ledr = 0; |
DiondeGreef | 2:a64e3c37b571 | 69 | Ledg = 1; |
DiondeGreef | 2:a64e3c37b571 | 70 | Ledb = 1; |
DiondeGreef | 2:a64e3c37b571 | 71 | break; |
DiondeGreef | 2:a64e3c37b571 | 72 | |
DiondeGreef | 2:a64e3c37b571 | 73 | case Open: |
DiondeGreef | 2:a64e3c37b571 | 74 | if(j <= 34){ |
DiondeGreef | 2:a64e3c37b571 | 75 | motorSpeed.write(0); |
DiondeGreef | 2:a64e3c37b571 | 76 | motorSpeed2.write(0); |
DiondeGreef | 2:a64e3c37b571 | 77 | j++; |
DiondeGreef | 2:a64e3c37b571 | 78 | } |
DiondeGreef | 2:a64e3c37b571 | 79 | else if (j == 35){ |
DiondeGreef | 2:a64e3c37b571 | 80 | motorSpeed.write(0); |
DiondeGreef | 2:a64e3c37b571 | 81 | motorSpeed2.write(1); |
DiondeGreef | 2:a64e3c37b571 | 82 | j++; |
DiondeGreef | 2:a64e3c37b571 | 83 | } |
DiondeGreef | 2:a64e3c37b571 | 84 | else if (j == 36){ |
DiondeGreef | 2:a64e3c37b571 | 85 | motorSpeed.write(0); |
DiondeGreef | 2:a64e3c37b571 | 86 | motorSpeed2.write(1); |
DiondeGreef | 2:a64e3c37b571 | 87 | j++; |
DiondeGreef | 2:a64e3c37b571 | 88 | } |
DiondeGreef | 2:a64e3c37b571 | 89 | else if (j == 37){ |
DiondeGreef | 2:a64e3c37b571 | 90 | motorSpeed.write(0); |
DiondeGreef | 2:a64e3c37b571 | 91 | motorSpeed2.write(1); |
DiondeGreef | 2:a64e3c37b571 | 92 | j++; |
DiondeGreef | 2:a64e3c37b571 | 93 | } |
DiondeGreef | 2:a64e3c37b571 | 94 | else if (j == 38){ |
DiondeGreef | 2:a64e3c37b571 | 95 | motorSpeed.write(0); |
DiondeGreef | 2:a64e3c37b571 | 96 | motorSpeed2.write(1); |
DiondeGreef | 2:a64e3c37b571 | 97 | j++; |
DiondeGreef | 2:a64e3c37b571 | 98 | } |
DiondeGreef | 2:a64e3c37b571 | 99 | else if (j == 39){ |
DiondeGreef | 2:a64e3c37b571 | 100 | motorSpeed.write(1); |
DiondeGreef | 2:a64e3c37b571 | 101 | motorSpeed2.write(1); |
DiondeGreef | 2:a64e3c37b571 | 102 | j = 0; |
DiondeGreef | 2:a64e3c37b571 | 103 | } |
DiondeGreef | 2:a64e3c37b571 | 104 | Ledr = 1; |
DiondeGreef | 2:a64e3c37b571 | 105 | Ledg = 1; |
DiondeGreef | 2:a64e3c37b571 | 106 | Ledb = 0; |
DiondeGreef | 2:a64e3c37b571 | 107 | break; |
DiondeGreef | 2:a64e3c37b571 | 108 | |
DiondeGreef | 2:a64e3c37b571 | 109 | default: |
DiondeGreef | 2:a64e3c37b571 | 110 | Ledr = 1; |
DiondeGreef | 2:a64e3c37b571 | 111 | Ledg = 0; |
DiondeGreef | 2:a64e3c37b571 | 112 | Ledb = 1; |
DiondeGreef | 2:a64e3c37b571 | 113 | } |
RoyvZ | 0:331597250051 | 114 | } |
RoyvZ | 0:331597250051 | 115 | |
DiondeGreef | 2:a64e3c37b571 | 116 | |
DiondeGreef | 2:a64e3c37b571 | 117 | |
RoyvZ | 0:331597250051 | 118 | int main() { |
DiondeGreef | 2:a64e3c37b571 | 119 | tick.attach(ProcessStateMachine, 0.0005); |
DiondeGreef | 2:a64e3c37b571 | 120 | Ledr = 1; |
DiondeGreef | 2:a64e3c37b571 | 121 | Ledg = 1; |
DiondeGreef | 2:a64e3c37b571 | 122 | Ledb = 1; |
RoyvZ | 0:331597250051 | 123 | |
DiondeGreef | 2:a64e3c37b571 | 124 | while (true) |
DiondeGreef | 2:a64e3c37b571 | 125 | { |
DiondeGreef | 2:a64e3c37b571 | 126 | Button1.rise(&Change); |
RoyvZ | 0:331597250051 | 127 | } |
DiondeGreef | 2:a64e3c37b571 | 128 | } |