Micromouse / Mbed 2 deprecated Main-codetest

Dependencies:   mbed

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?

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