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@9:76e4808df4cb, 2017-12-01 (annotated)

Committer:
x58alex41
Date:
Fri Dec 01 21:20:29 2017 +0000
Revision:
9:76e4808df4cb
Parent:
7:edd065946e9b
Child:
10:4a825f818432
motor problems;

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 7:edd065946e9b 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 9:76e4808df4cb 82 if(cycle_speed_M1<0){ //Can't go under 0 either
x58alex41 9:76e4808df4cb 83 // pc.printf("M1: %8.8f\n",cycle_speed_M1);
x58alex41 0:9c49bdc7e402 84 cycle_speed_M1=0;
x58alex41 9:76e4808df4cb 85 }
x58alex41 0:9c49bdc7e402 86 if(cycle_speed_M2>1)
x58alex41 0:9c49bdc7e402 87 cycle_speed_M2=1;
x58alex41 9:76e4808df4cb 88 if(cycle_speed_M2<0){
x58alex41 9:76e4808df4cb 89 //pc.printf("M2: %8.8f\n",cycle_speed_M2);
x58alex41 0:9c49bdc7e402 90 cycle_speed_M2=0;
x58alex41 9:76e4808df4cb 91 }
x58alex41 9:76e4808df4cb 92
x58alex41 9:76e4808df4cb 93 //pc.printf("m1: %8.8f\n",cycle_speed_M1);
x58alex41 9:76e4808df4cb 94 //pc.printf("m2: %8.8f\n",cycle_speed_M2);
x58alex41 0:9c49bdc7e402 95 //adjusting delays based off PID_error corrections
x58alex41 0:9c49bdc7e402 96 pulse_M1 = (pulse_cycle-pulse_cycle*(1-cycle_speed_M1));
x58alex41 0:9c49bdc7e402 97 period_M1 = (pulse_cycle-pulse_cycle*cycle_speed_M1);
x58alex41 0:9c49bdc7e402 98
x58alex41 0:9c49bdc7e402 99
x58alex41 0:9c49bdc7e402 100 pulse_M2 = (pulse_cycle-pulse_cycle*(1-cycle_speed_M2));
x58alex41 0:9c49bdc7e402 101 period_M2 = (pulse_cycle-pulse_cycle*cycle_speed_M2);
x58alex41 0:9c49bdc7e402 102
x58alex41 9:76e4808df4cb 103 //pc.printf("pulse2: %8.8f\n", pulse_M1);
x58alex41 9:76e4808df4cb 104 // pc.printf("period2: %8.8f\n", period_M1);
x58alex41 0:9c49bdc7e402 105 //controlling the speed for motor 1
x58alex41 0:9c49bdc7e402 106 if(High_M1) {
x58alex41 0:9c49bdc7e402 107 High_M1=pulse_M1 > T_M1.read_ms();
x58alex41 9:76e4808df4cb 108 //pc.printf("timer m1 move: %8.8f\n",T_M1.read_ms());
x58alex41 0:9c49bdc7e402 109 if(!High_M1) { //pulse has finished and going LOW
x58alex41 0:9c49bdc7e402 110 dir_control_M1(stall);
x58alex41 9:76e4808df4cb 111 pc.printf("timer m1 stall: %f\n",T_M1.read_ms());
x58alex41 0:9c49bdc7e402 112 T_M1.reset(); //start the delay between pulse (note that it continues counting)
x58alex41 0:9c49bdc7e402 113 }
x58alex41 0:9c49bdc7e402 114 } else {
x58alex41 0:9c49bdc7e402 115 High_M1=period_M1 < T_M1.read_ms();
x58alex41 0:9c49bdc7e402 116 if(High_M1) { //delay has finished and going HIGH
x58alex41 0:9c49bdc7e402 117 dir_control_M1(dir_M1);
x58alex41 0:9c49bdc7e402 118 T_M1.reset(); //start the pulse and its clock
x58alex41 0:9c49bdc7e402 119 }
x58alex41 0:9c49bdc7e402 120 }
x58alex41 0:9c49bdc7e402 121
x58alex41 0:9c49bdc7e402 122 //repeated for Motor 2
x58alex41 0:9c49bdc7e402 123 if(High_M2) {
x58alex41 0:9c49bdc7e402 124 High_M2=pulse_M2 > T_M2.read_ms();
x58alex41 0:9c49bdc7e402 125 if(!High_M2) {
x58alex41 0:9c49bdc7e402 126 dir_control_M2(stall);
x58alex41 0:9c49bdc7e402 127 T_M2.reset();
x58alex41 0:9c49bdc7e402 128 }
x58alex41 0:9c49bdc7e402 129 } else {
x58alex41 0:9c49bdc7e402 130 High_M2=period_M2 < T_M2.read_ms();
x58alex41 0:9c49bdc7e402 131 if(High_M2) {
x58alex41 0:9c49bdc7e402 132 dir_control_M2(dir_M2);
x58alex41 0:9c49bdc7e402 133 T_M2.reset();
x58alex41 0:9c49bdc7e402 134 }
x58alex41 0:9c49bdc7e402 135 }
x58alex41 7:edd065946e9b 136
x58alex41 0:9c49bdc7e402 137 }
x58alex41 0:9c49bdc7e402 138 T.stop();
x58alex41 0:9c49bdc7e402 139 T_M1.stop();
x58alex41 0:9c49bdc7e402 140 T_M2.stop();
x58alex41 7:edd065946e9b 141
x58alex41 0:9c49bdc7e402 142 }
x58alex41 0:9c49bdc7e402 143
x58alex41 7:edd065946e9b 144 void sharp_right(int delay, int cycle_speed)
x58alex41 7:edd065946e9b 145 {
x58alex41 7:edd065946e9b 146 move(cycle_speed, delay, forward, backward, PID);
x58alex41 7:edd065946e9b 147 move(cycle_speed, delay, forward, backward, PID);
x58alex41 7:edd065946e9b 148 }
x58alex41 7:edd065946e9b 149 void sharp_left(int delay)
x58alex41 7:edd065946e9b 150 {
x58alex41 7:edd065946e9b 151
x58alex41 7:edd065946e9b 152 }
x58alex41 7:edd065946e9b 153
x58alex41 7:edd065946e9b 154
x58alex41 7:edd065946e9b 155