Dion de Greef / Mbed 2 deprecated feed_forward

Nu werkt het wel, opdracht 1 is af

Dependencies:   MODSERIAL mbed QEI feed_forward

Fork of feed_forward by Roy van Zijl

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?

UserRevisionLine numberNew 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 }