Filters hebben de goede waarden. Safety stop werkt nog niet.
Dependencies: Encoder HIDScope MODSERIAL mbed
Fork of Total_code_v1_Gr13 by
main.cpp@1:7d5e6bc2b314, 2015-10-19 (annotated)
- Committer:
- riannebulthuis
- Date:
- Mon Oct 19 13:44:23 2015 +0000
- Revision:
- 1:7d5e6bc2b314
- Parent:
- 0:65ab9f79a4cc
- Child:
- 2:866a8a9f2b93
homeposition in 3 stappen
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
arunr | 0:65ab9f79a4cc | 1 | #include "mbed.h" |
arunr | 0:65ab9f79a4cc | 2 | #include "encoder.h" |
arunr | 0:65ab9f79a4cc | 3 | #include "HIDScope.h" |
riannebulthuis | 1:7d5e6bc2b314 | 4 | #include "QEI.h" |
riannebulthuis | 1:7d5e6bc2b314 | 5 | #include "MODSERIAL.h" |
arunr | 0:65ab9f79a4cc | 6 | |
arunr | 0:65ab9f79a4cc | 7 | DigitalOut motor1_direction(D4); |
arunr | 0:65ab9f79a4cc | 8 | PwmOut motor1_speed(D5); |
arunr | 0:65ab9f79a4cc | 9 | PwmOut led(D9); |
arunr | 0:65ab9f79a4cc | 10 | DigitalIn button_1(PTC6); //counterclockwise |
arunr | 0:65ab9f79a4cc | 11 | DigitalIn button_2(PTA4); //clockwise |
arunr | 0:65ab9f79a4cc | 12 | Encoder motor1(D12,D13); |
arunr | 0:65ab9f79a4cc | 13 | HIDScope scope(1); |
arunr | 0:65ab9f79a4cc | 14 | |
arunr | 0:65ab9f79a4cc | 15 | |
arunr | 0:65ab9f79a4cc | 16 | const int pressed = 0; |
arunr | 0:65ab9f79a4cc | 17 | |
arunr | 0:65ab9f79a4cc | 18 | double H; |
arunr | 0:65ab9f79a4cc | 19 | double P; |
arunr | 0:65ab9f79a4cc | 20 | double D; |
arunr | 0:65ab9f79a4cc | 21 | |
arunr | 0:65ab9f79a4cc | 22 | |
arunr | 0:65ab9f79a4cc | 23 | void sethome(){ |
arunr | 0:65ab9f79a4cc | 24 | motor1.setPosition(0); |
arunr | 0:65ab9f79a4cc | 25 | H = motor1.getPosition(); |
arunr | 0:65ab9f79a4cc | 26 | } |
arunr | 0:65ab9f79a4cc | 27 | |
arunr | 0:65ab9f79a4cc | 28 | void move_motor1_ccw (){ |
arunr | 0:65ab9f79a4cc | 29 | motor1_direction = 0; |
arunr | 0:65ab9f79a4cc | 30 | motor1_speed = 0.8; |
arunr | 0:65ab9f79a4cc | 31 | } |
arunr | 0:65ab9f79a4cc | 32 | |
arunr | 0:65ab9f79a4cc | 33 | void move_motor1_cw (){ |
arunr | 0:65ab9f79a4cc | 34 | motor1_direction = 1; |
arunr | 0:65ab9f79a4cc | 35 | motor1_speed = 0.8; |
arunr | 0:65ab9f79a4cc | 36 | } |
arunr | 0:65ab9f79a4cc | 37 | |
arunr | 0:65ab9f79a4cc | 38 | void movetohome(){ |
arunr | 0:65ab9f79a4cc | 39 | P = motor1.getPosition(); |
arunr | 0:65ab9f79a4cc | 40 | D = (P - H); |
arunr | 0:65ab9f79a4cc | 41 | |
arunr | 0:65ab9f79a4cc | 42 | if (D == 0){ |
arunr | 0:65ab9f79a4cc | 43 | motor1_speed = 0; |
arunr | 0:65ab9f79a4cc | 44 | } |
arunr | 0:65ab9f79a4cc | 45 | else if (D > 0){ |
arunr | 0:65ab9f79a4cc | 46 | move_motor1_cw(); |
arunr | 0:65ab9f79a4cc | 47 | } |
arunr | 0:65ab9f79a4cc | 48 | else if (D < 0){ |
arunr | 0:65ab9f79a4cc | 49 | move_motor1_ccw(); |
arunr | 0:65ab9f79a4cc | 50 | } |
arunr | 0:65ab9f79a4cc | 51 | } |
arunr | 0:65ab9f79a4cc | 52 | |
arunr | 0:65ab9f79a4cc | 53 | void move_motor1() |
arunr | 0:65ab9f79a4cc | 54 | { |
arunr | 0:65ab9f79a4cc | 55 | if (button_1 == pressed) { |
arunr | 0:65ab9f79a4cc | 56 | move_motor1_cw (); |
riannebulthuis | 1:7d5e6bc2b314 | 57 | } else if (button_2 == pressed) { |
riannebulthuis | 1:7d5e6bc2b314 | 58 | move_motor1_ccw (); |
riannebulthuis | 1:7d5e6bc2b314 | 59 | } else { |
riannebulthuis | 1:7d5e6bc2b314 | 60 | motor1_speed = 0; |
arunr | 0:65ab9f79a4cc | 61 | } |
arunr | 0:65ab9f79a4cc | 62 | } |
arunr | 0:65ab9f79a4cc | 63 | |
arunr | 0:65ab9f79a4cc | 64 | void read_encoder1 () // aflezen van encoder via hidscope?? |
arunr | 0:65ab9f79a4cc | 65 | { |
arunr | 0:65ab9f79a4cc | 66 | scope.set(0,motor1.getPosition()); |
arunr | 0:65ab9f79a4cc | 67 | led.write(motor1.getPosition()/100.0); |
arunr | 0:65ab9f79a4cc | 68 | scope.send(); |
arunr | 0:65ab9f79a4cc | 69 | wait(0.2f); |
arunr | 0:65ab9f79a4cc | 70 | } |
arunr | 0:65ab9f79a4cc | 71 | |
arunr | 0:65ab9f79a4cc | 72 | int main() |
arunr | 0:65ab9f79a4cc | 73 | { |
arunr | 0:65ab9f79a4cc | 74 | while (true) { |
riannebulthuis | 1:7d5e6bc2b314 | 75 | pc.baud(9600); //pc baud rate van de computer |
riannebulthuis | 1:7d5e6bc2b314 | 76 | |
riannebulthuis | 1:7d5e6bc2b314 | 77 | switch (state) { //zorgt voor het in goede volgorde uitvoeren van de cases |
riannebulthuis | 1:7d5e6bc2b314 | 78 | |
riannebulthuis | 1:7d5e6bc2b314 | 79 | case gotohomeposition: //positie op 0 zetten voor arm 1 |
riannebulthuis | 1:7d5e6bc2b314 | 80 | { |
riannebulthuis | 1:7d5e6bc2b314 | 81 | read_encoder1(); |
riannebulthuis | 1:7d5e6bc2b314 | 82 | sethome(); |
riannebulthuis | 1:7d5e6bc2b314 | 83 | state = move_motor; |
riannebulthuis | 1:7d5e6bc2b314 | 84 | break; |
riannebulthuis | 1:7d5e6bc2b314 | 85 | } |
riannebulthuis | 1:7d5e6bc2b314 | 86 | |
riannebulthuis | 1:7d5e6bc2b314 | 87 | case move_motor: //motor kan cw en ccw bewegen a.d.h.v. buttons |
riannebulthuis | 1:7d5e6bc2b314 | 88 | { |
riannebulthuis | 1:7d5e6bc2b314 | 89 | move_motor1 (); |
riannebulthuis | 1:7d5e6bc2b314 | 90 | state = homeposition; |
riannebulthuis | 1:7d5e6bc2b314 | 91 | break; |
riannebulthuis | 1:7d5e6bc2b314 | 92 | } |
riannebulthuis | 1:7d5e6bc2b314 | 93 | |
riannebulthuis | 1:7d5e6bc2b314 | 94 | case backtohomeposition: //motor gaat terug naar homeposition |
riannebulthuis | 1:7d5e6bc2b314 | 95 | { |
riannebulthuis | 1:7d5e6bc2b314 | 96 | movetohome(); |
riannebulthuis | 1:7d5e6bc2b314 | 97 | break; |
riannebulthuis | 1:7d5e6bc2b314 | 98 | } |
arunr | 0:65ab9f79a4cc | 99 | } |