Micromouse / Mbed 2 deprecated Main-codetest

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@0:9c49bdc7e402, 2017-11-22 (annotated)

Committer:
x58alex41
Date:
Wed Nov 22 02:18:07 2017 +0000
Revision:
0:9c49bdc7e402
Child:
4:c8929825a5f4
test;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
x58alex41 0:9c49bdc7e402 1 #include "controls.h"
x58alex41 0:9c49bdc7e402 2
x58alex41 0:9c49bdc7e402 3 void dir_control_M1(controls dir)
x58alex41 0:9c49bdc7e402 4 {
x58alex41 0:9c49bdc7e402 5 switch(dir) {
x58alex41 0:9c49bdc7e402 6 case forward:
x58alex41 0:9c49bdc7e402 7 M1F=1;
x58alex41 0:9c49bdc7e402 8 M1B=0;
x58alex41 0:9c49bdc7e402 9 break;
x58alex41 0:9c49bdc7e402 10 case backward:
x58alex41 0:9c49bdc7e402 11 M1F=0;
x58alex41 0:9c49bdc7e402 12 M1B=1;
x58alex41 0:9c49bdc7e402 13 break;
x58alex41 0:9c49bdc7e402 14 case brake:
x58alex41 0:9c49bdc7e402 15 M1F=1;
x58alex41 0:9c49bdc7e402 16 M1B=1;
x58alex41 0:9c49bdc7e402 17 break;
x58alex41 0:9c49bdc7e402 18 case stall:
x58alex41 0:9c49bdc7e402 19 default:
x58alex41 0:9c49bdc7e402 20 M1F=0;
x58alex41 0:9c49bdc7e402 21 M1B=0;
x58alex41 0:9c49bdc7e402 22 break;
x58alex41 0:9c49bdc7e402 23 }
x58alex41 0:9c49bdc7e402 24 }
x58alex41 0:9c49bdc7e402 25 void dir_control_M2(controls dir)
x58alex41 0:9c49bdc7e402 26 {
x58alex41 0:9c49bdc7e402 27 switch(dir) {
x58alex41 0:9c49bdc7e402 28 case forward:
x58alex41 0:9c49bdc7e402 29 M2F=1;
x58alex41 0:9c49bdc7e402 30 M2B=0;
x58alex41 0:9c49bdc7e402 31 break;
x58alex41 0:9c49bdc7e402 32 case backward:
x58alex41 0:9c49bdc7e402 33 M2F=0;
x58alex41 0:9c49bdc7e402 34 M2B=1;
x58alex41 0:9c49bdc7e402 35 break;
x58alex41 0:9c49bdc7e402 36 case brake:
x58alex41 0:9c49bdc7e402 37 M2F=1;
x58alex41 0:9c49bdc7e402 38 M2B=1;
x58alex41 0:9c49bdc7e402 39 break;
x58alex41 0:9c49bdc7e402 40 case stall:
x58alex41 0:9c49bdc7e402 41 default:
x58alex41 0:9c49bdc7e402 42 M2F=0;
x58alex41 0:9c49bdc7e402 43 M2B=0;
x58alex41 0:9c49bdc7e402 44 break;
x58alex41 0:9c49bdc7e402 45 }
x58alex41 0:9c49bdc7e402 46 }
x58alex41 0:9c49bdc7e402 47
x58alex41 0:9c49bdc7e402 48 const double pulse_cycle = 10; // ms
x58alex41 0:9c49bdc7e402 49
x58alex41 0:9c49bdc7e402 50 void move(double cycle_speed, int duration_ms, controls dir_M1, controls dir_M2, double (*PID_error)()) // cycle_speed from 0 - 1
x58alex41 0:9c49bdc7e402 51 {
x58alex41 0:9c49bdc7e402 52 Timer T;
x58alex41 0:9c49bdc7e402 53 Timer T_M1;
x58alex41 0:9c49bdc7e402 54 Timer T_M2;
x58alex41 0:9c49bdc7e402 55
x58alex41 0:9c49bdc7e402 56 //start moving first
x58alex41 0:9c49bdc7e402 57 bool High_M1=true;
x58alex41 0:9c49bdc7e402 58 bool High_M2=true;
x58alex41 0:9c49bdc7e402 59
x58alex41 0:9c49bdc7e402 60 double pulse_M1;
x58alex41 0:9c49bdc7e402 61 double period_M1;
x58alex41 0:9c49bdc7e402 62 double pulse_M2;
x58alex41 0:9c49bdc7e402 63 double period_M2;
x58alex41 0:9c49bdc7e402 64
x58alex41 0:9c49bdc7e402 65 double cycle_speed_M1;
x58alex41 0:9c49bdc7e402 66 double cycle_speed_M2;
x58alex41 0:9c49bdc7e402 67
x58alex41 0:9c49bdc7e402 68 dir_control_M1(dir_M1);
x58alex41 0:9c49bdc7e402 69 dir_control_M2(dir_M2);
x58alex41 0:9c49bdc7e402 70
x58alex41 0:9c49bdc7e402 71 T.start();
x58alex41 0:9c49bdc7e402 72 T_M1.start();
x58alex41 0:9c49bdc7e402 73 T_M2.start();
x58alex41 0:9c49bdc7e402 74
x58alex41 0:9c49bdc7e402 75 while(T.read_ms()<(duration_ms)) {
x58alex41 0:9c49bdc7e402 76
x58alex41 0:9c49bdc7e402 77 cycle_speed_M1= cycle_speed - (*PID_error)(); //PID_error() should be under 1 most/all of the time ideally
x58alex41 0:9c49bdc7e402 78 cycle_speed_M2= cycle_speed + (*PID_error)();
x58alex41 0:9c49bdc7e402 79
x58alex41 0:9c49bdc7e402 80 if(cycle_speed_M1>1) // Can't go over 1 as that is the max speed
x58alex41 0:9c49bdc7e402 81 cycle_speed_M1=1;
x58alex41 0:9c49bdc7e402 82 if(cycle_speed_M1<0) //Can't go under o either
x58alex41 0:9c49bdc7e402 83 cycle_speed_M1=0;
x58alex41 0:9c49bdc7e402 84 if(cycle_speed_M2>1)
x58alex41 0:9c49bdc7e402 85 cycle_speed_M2=1;
x58alex41 0:9c49bdc7e402 86 if(cycle_speed_M2<0)
x58alex41 0:9c49bdc7e402 87 cycle_speed_M2=0;
x58alex41 0:9c49bdc7e402 88
x58alex41 0:9c49bdc7e402 89
x58alex41 0:9c49bdc7e402 90 /*
x58alex41 0:9c49bdc7e402 91 //adjusting delays based off PID_error corrections
x58alex41 0:9c49bdc7e402 92 pulse_M1 = (pulse_cycle-pulse_cycle*(1-cycle_speed_M1));
x58alex41 0:9c49bdc7e402 93 period_M1 = (pulse_cycle-pulse_cycle*cycle_speed_M1);
x58alex41 0:9c49bdc7e402 94
x58alex41 0:9c49bdc7e402 95
x58alex41 0:9c49bdc7e402 96 pulse_M2 = (pulse_cycle-pulse_cycle*(1-cycle_speed_M2));
x58alex41 0:9c49bdc7e402 97 period_M2 = (pulse_cycle-pulse_cycle*cycle_speed_M2);
x58alex41 0:9c49bdc7e402 98
x58alex41 0:9c49bdc7e402 99 //controlling the speed for motor 1
x58alex41 0:9c49bdc7e402 100 if(High_M1) {
x58alex41 0:9c49bdc7e402 101 High_M1=pulse_M1 > T_M1.read_ms();
x58alex41 0:9c49bdc7e402 102 if(!High_M1) { //pulse has finished and going LOW
x58alex41 0:9c49bdc7e402 103 dir_control_M1(stall);
x58alex41 0:9c49bdc7e402 104 T_M1.reset(); //start the delay between pulse (note that it continues counting)
x58alex41 0:9c49bdc7e402 105 }
x58alex41 0:9c49bdc7e402 106 } else {
x58alex41 0:9c49bdc7e402 107 High_M1=period_M1 < T_M1.read_ms();
x58alex41 0:9c49bdc7e402 108 if(High_M1) { //delay has finished and going HIGH
x58alex41 0:9c49bdc7e402 109 dir_control_M1(dir_M1);
x58alex41 0:9c49bdc7e402 110 T_M1.reset(); //start the pulse and its clock
x58alex41 0:9c49bdc7e402 111 }
x58alex41 0:9c49bdc7e402 112 }
x58alex41 0:9c49bdc7e402 113
x58alex41 0:9c49bdc7e402 114 //repeated for Motor 2
x58alex41 0:9c49bdc7e402 115 if(High_M2) {
x58alex41 0:9c49bdc7e402 116 High_M2=pulse_M2 > T_M2.read_ms();
x58alex41 0:9c49bdc7e402 117 if(!High_M2) {
x58alex41 0:9c49bdc7e402 118 dir_control_M2(stall);
x58alex41 0:9c49bdc7e402 119 T_M2.reset();
x58alex41 0:9c49bdc7e402 120 }
x58alex41 0:9c49bdc7e402 121 } else {
x58alex41 0:9c49bdc7e402 122 High_M2=period_M2 < T_M2.read_ms();
x58alex41 0:9c49bdc7e402 123 if(High_M2) {
x58alex41 0:9c49bdc7e402 124 dir_control_M2(dir_M2);
x58alex41 0:9c49bdc7e402 125 T_M2.reset();
x58alex41 0:9c49bdc7e402 126 }
x58alex41 0:9c49bdc7e402 127 }
x58alex41 0:9c49bdc7e402 128 }
x58alex41 0:9c49bdc7e402 129 T.stop();
x58alex41 0:9c49bdc7e402 130 T_M1.stop();
x58alex41 0:9c49bdc7e402 131 T_M2.stop();
x58alex41 0:9c49bdc7e402 132 */
x58alex41 0:9c49bdc7e402 133 }
x58alex41 0:9c49bdc7e402 134