Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed
controls.cpp@8:45797dcd8b66, 2017-12-01 (annotated)
- Committer:
- x58alex41
- Date:
- Fri Dec 01 20:07:48 2017 +0000
- Revision:
- 8:45797dcd8b66
- Parent:
- 7:edd065946e9b
stuf ffor later;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
x58alex41 | 0:9c49bdc7e402 | 1 | #include "controls.h" |
x58alex41 | 8:45797dcd8b66 | 2 | #include "controller.h" |
x58alex41 | 8:45797dcd8b66 | 3 | |
x58alex41 | 8:45797dcd8b66 | 4 | class controller; |
x58alex41 | 0:9c49bdc7e402 | 5 | |
x58alex41 | 0:9c49bdc7e402 | 6 | void dir_control_M1(controls dir) |
x58alex41 | 0:9c49bdc7e402 | 7 | { |
x58alex41 | 0:9c49bdc7e402 | 8 | switch(dir) { |
x58alex41 | 0:9c49bdc7e402 | 9 | case forward: |
x58alex41 | 0:9c49bdc7e402 | 10 | M1F=1; |
x58alex41 | 0:9c49bdc7e402 | 11 | M1B=0; |
x58alex41 | 0:9c49bdc7e402 | 12 | break; |
x58alex41 | 0:9c49bdc7e402 | 13 | case backward: |
x58alex41 | 0:9c49bdc7e402 | 14 | M1F=0; |
x58alex41 | 0:9c49bdc7e402 | 15 | M1B=1; |
x58alex41 | 0:9c49bdc7e402 | 16 | break; |
x58alex41 | 0:9c49bdc7e402 | 17 | case brake: |
x58alex41 | 0:9c49bdc7e402 | 18 | M1F=1; |
x58alex41 | 0:9c49bdc7e402 | 19 | M1B=1; |
x58alex41 | 0:9c49bdc7e402 | 20 | break; |
x58alex41 | 0:9c49bdc7e402 | 21 | case stall: |
x58alex41 | 0:9c49bdc7e402 | 22 | default: |
x58alex41 | 0:9c49bdc7e402 | 23 | M1F=0; |
x58alex41 | 0:9c49bdc7e402 | 24 | M1B=0; |
x58alex41 | 0:9c49bdc7e402 | 25 | break; |
x58alex41 | 0:9c49bdc7e402 | 26 | } |
x58alex41 | 0:9c49bdc7e402 | 27 | } |
x58alex41 | 0:9c49bdc7e402 | 28 | void dir_control_M2(controls dir) |
x58alex41 | 0:9c49bdc7e402 | 29 | { |
x58alex41 | 0:9c49bdc7e402 | 30 | switch(dir) { |
x58alex41 | 0:9c49bdc7e402 | 31 | case forward: |
x58alex41 | 0:9c49bdc7e402 | 32 | M2F=1; |
x58alex41 | 0:9c49bdc7e402 | 33 | M2B=0; |
x58alex41 | 0:9c49bdc7e402 | 34 | break; |
x58alex41 | 0:9c49bdc7e402 | 35 | case backward: |
x58alex41 | 0:9c49bdc7e402 | 36 | M2F=0; |
x58alex41 | 0:9c49bdc7e402 | 37 | M2B=1; |
x58alex41 | 0:9c49bdc7e402 | 38 | break; |
x58alex41 | 0:9c49bdc7e402 | 39 | case brake: |
x58alex41 | 0:9c49bdc7e402 | 40 | M2F=1; |
x58alex41 | 0:9c49bdc7e402 | 41 | M2B=1; |
x58alex41 | 0:9c49bdc7e402 | 42 | break; |
x58alex41 | 0:9c49bdc7e402 | 43 | case stall: |
x58alex41 | 0:9c49bdc7e402 | 44 | default: |
x58alex41 | 0:9c49bdc7e402 | 45 | M2F=0; |
x58alex41 | 0:9c49bdc7e402 | 46 | M2B=0; |
x58alex41 | 0:9c49bdc7e402 | 47 | break; |
x58alex41 | 0:9c49bdc7e402 | 48 | } |
x58alex41 | 0:9c49bdc7e402 | 49 | } |
x58alex41 | 0:9c49bdc7e402 | 50 | |
x58alex41 | 0:9c49bdc7e402 | 51 | const double pulse_cycle = 10; // ms |
x58alex41 | 0:9c49bdc7e402 | 52 | |
x58alex41 | 8:45797dcd8b66 | 53 | void move(double cycle_speed, *controller cntlr, controls dir_M1, controls dir_M2, double (*PID_error)()) // cycle_speed from 0 - 1 |
x58alex41 | 0:9c49bdc7e402 | 54 | { |
x58alex41 | 0:9c49bdc7e402 | 55 | Timer T; |
x58alex41 | 0:9c49bdc7e402 | 56 | Timer T_M1; |
x58alex41 | 0:9c49bdc7e402 | 57 | Timer T_M2; |
x58alex41 | 0:9c49bdc7e402 | 58 | |
x58alex41 | 0:9c49bdc7e402 | 59 | //start moving first |
x58alex41 | 0:9c49bdc7e402 | 60 | bool High_M1=true; |
x58alex41 | 0:9c49bdc7e402 | 61 | bool High_M2=true; |
x58alex41 | 0:9c49bdc7e402 | 62 | |
x58alex41 | 0:9c49bdc7e402 | 63 | double pulse_M1; |
x58alex41 | 0:9c49bdc7e402 | 64 | double period_M1; |
x58alex41 | 0:9c49bdc7e402 | 65 | double pulse_M2; |
x58alex41 | 0:9c49bdc7e402 | 66 | double period_M2; |
x58alex41 | 0:9c49bdc7e402 | 67 | |
x58alex41 | 0:9c49bdc7e402 | 68 | double cycle_speed_M1; |
x58alex41 | 0:9c49bdc7e402 | 69 | double cycle_speed_M2; |
x58alex41 | 0:9c49bdc7e402 | 70 | |
x58alex41 | 0:9c49bdc7e402 | 71 | dir_control_M1(dir_M1); |
x58alex41 | 0:9c49bdc7e402 | 72 | dir_control_M2(dir_M2); |
x58alex41 | 0:9c49bdc7e402 | 73 | |
x58alex41 | 0:9c49bdc7e402 | 74 | T.start(); |
x58alex41 | 0:9c49bdc7e402 | 75 | T_M1.start(); |
x58alex41 | 0:9c49bdc7e402 | 76 | T_M2.start(); |
x58alex41 | 0:9c49bdc7e402 | 77 | |
x58alex41 | 8:45797dcd8b66 | 78 | while(counterM1 > cntlr->get_turn_cycles() || get_event() ) { |
x58alex41 | 0:9c49bdc7e402 | 79 | |
x58alex41 | 0:9c49bdc7e402 | 80 | cycle_speed_M1= cycle_speed - (*PID_error)(); //PID_error() should be under 1 most/all of the time ideally |
x58alex41 | 0:9c49bdc7e402 | 81 | cycle_speed_M2= cycle_speed + (*PID_error)(); |
x58alex41 | 7:edd065946e9b | 82 | |
x58alex41 | 0:9c49bdc7e402 | 83 | if(cycle_speed_M1>1) // Can't go over 1 as that is the max speed |
x58alex41 | 0:9c49bdc7e402 | 84 | cycle_speed_M1=1; |
x58alex41 | 7:edd065946e9b | 85 | if(cycle_speed_M1<0) //Can't go under 0 either |
x58alex41 | 0:9c49bdc7e402 | 86 | cycle_speed_M1=0; |
x58alex41 | 0:9c49bdc7e402 | 87 | if(cycle_speed_M2>1) |
x58alex41 | 0:9c49bdc7e402 | 88 | cycle_speed_M2=1; |
x58alex41 | 0:9c49bdc7e402 | 89 | if(cycle_speed_M2<0) |
x58alex41 | 0:9c49bdc7e402 | 90 | cycle_speed_M2=0; |
x58alex41 | 7:edd065946e9b | 91 | |
x58alex41 | 6:688449345fff | 92 | |
x58alex41 | 0:9c49bdc7e402 | 93 | //adjusting delays based off PID_error corrections |
x58alex41 | 0:9c49bdc7e402 | 94 | pulse_M1 = (pulse_cycle-pulse_cycle*(1-cycle_speed_M1)); |
x58alex41 | 0:9c49bdc7e402 | 95 | period_M1 = (pulse_cycle-pulse_cycle*cycle_speed_M1); |
x58alex41 | 0:9c49bdc7e402 | 96 | |
x58alex41 | 0:9c49bdc7e402 | 97 | |
x58alex41 | 0:9c49bdc7e402 | 98 | pulse_M2 = (pulse_cycle-pulse_cycle*(1-cycle_speed_M2)); |
x58alex41 | 0:9c49bdc7e402 | 99 | period_M2 = (pulse_cycle-pulse_cycle*cycle_speed_M2); |
x58alex41 | 0:9c49bdc7e402 | 100 | |
x58alex41 | 0:9c49bdc7e402 | 101 | //controlling the speed for motor 1 |
x58alex41 | 0:9c49bdc7e402 | 102 | if(High_M1) { |
x58alex41 | 0:9c49bdc7e402 | 103 | High_M1=pulse_M1 > T_M1.read_ms(); |
x58alex41 | 0:9c49bdc7e402 | 104 | if(!High_M1) { //pulse has finished and going LOW |
x58alex41 | 0:9c49bdc7e402 | 105 | dir_control_M1(stall); |
x58alex41 | 0:9c49bdc7e402 | 106 | T_M1.reset(); //start the delay between pulse (note that it continues counting) |
x58alex41 | 0:9c49bdc7e402 | 107 | } |
x58alex41 | 0:9c49bdc7e402 | 108 | } else { |
x58alex41 | 0:9c49bdc7e402 | 109 | High_M1=period_M1 < T_M1.read_ms(); |
x58alex41 | 0:9c49bdc7e402 | 110 | if(High_M1) { //delay has finished and going HIGH |
x58alex41 | 0:9c49bdc7e402 | 111 | dir_control_M1(dir_M1); |
x58alex41 | 0:9c49bdc7e402 | 112 | T_M1.reset(); //start the pulse and its clock |
x58alex41 | 0:9c49bdc7e402 | 113 | } |
x58alex41 | 0:9c49bdc7e402 | 114 | } |
x58alex41 | 0:9c49bdc7e402 | 115 | |
x58alex41 | 0:9c49bdc7e402 | 116 | //repeated for Motor 2 |
x58alex41 | 0:9c49bdc7e402 | 117 | if(High_M2) { |
x58alex41 | 0:9c49bdc7e402 | 118 | High_M2=pulse_M2 > T_M2.read_ms(); |
x58alex41 | 0:9c49bdc7e402 | 119 | if(!High_M2) { |
x58alex41 | 0:9c49bdc7e402 | 120 | dir_control_M2(stall); |
x58alex41 | 0:9c49bdc7e402 | 121 | T_M2.reset(); |
x58alex41 | 0:9c49bdc7e402 | 122 | } |
x58alex41 | 0:9c49bdc7e402 | 123 | } else { |
x58alex41 | 0:9c49bdc7e402 | 124 | High_M2=period_M2 < T_M2.read_ms(); |
x58alex41 | 0:9c49bdc7e402 | 125 | if(High_M2) { |
x58alex41 | 0:9c49bdc7e402 | 126 | dir_control_M2(dir_M2); |
x58alex41 | 0:9c49bdc7e402 | 127 | T_M2.reset(); |
x58alex41 | 0:9c49bdc7e402 | 128 | } |
x58alex41 | 0:9c49bdc7e402 | 129 | } |
x58alex41 | 7:edd065946e9b | 130 | |
x58alex41 | 0:9c49bdc7e402 | 131 | } |
x58alex41 | 0:9c49bdc7e402 | 132 | T.stop(); |
x58alex41 | 0:9c49bdc7e402 | 133 | T_M1.stop(); |
x58alex41 | 0:9c49bdc7e402 | 134 | T_M2.stop(); |
x58alex41 | 7:edd065946e9b | 135 | |
x58alex41 | 0:9c49bdc7e402 | 136 | } |
x58alex41 | 0:9c49bdc7e402 | 137 | |
x58alex41 | 7:edd065946e9b | 138 | |
x58alex41 | 7:edd065946e9b | 139 |