DECS @UNIST / Mbed 2 deprecated Anybaro_ver_7

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

MotorControl.cpp@5:6bb52b2a79bf, 2018-12-26 (annotated)

Committer:
whutsup
Date:
Wed Dec 26 10:58:58 2018 +0000
Revision:
5:6bb52b2a79bf
Parent:
4:bf4ad2079096
Child:
8:a435e7aa7a02
updated 181226

Who changed what in which revision?

UserRevisionLine numberNew contents of line
whutsup 4:bf4ad2079096 1 #include "mbed.h"
whutsup 4:bf4ad2079096 2
whutsup 4:bf4ad2079096 3 extern Serial bt;
whutsup 4:bf4ad2079096 4 extern Serial pc;
whutsup 4:bf4ad2079096 5
whutsup 4:bf4ad2079096 6 DigitalOut enable(PB_12);
whutsup 4:bf4ad2079096 7 PwmOut p1(PB_13);
whutsup 4:bf4ad2079096 8 PwmOut p2(PB_14);
whutsup 4:bf4ad2079096 9
whutsup 4:bf4ad2079096 10 extern InterruptIn up;
whutsup 4:bf4ad2079096 11 extern InterruptIn down;
whutsup 4:bf4ad2079096 12
whutsup 4:bf4ad2079096 13 extern AnalogIn disSensor;
whutsup 4:bf4ad2079096 14
whutsup 4:bf4ad2079096 15 extern Serial bt;
whutsup 4:bf4ad2079096 16 extern Ticker timer1;
whutsup 4:bf4ad2079096 17 extern Ticker timer3;
whutsup 4:bf4ad2079096 18
whutsup 4:bf4ad2079096 19 extern int targetDis;
whutsup 4:bf4ad2079096 20 extern int milLoop;
whutsup 4:bf4ad2079096 21 extern int onewayNum;
whutsup 4:bf4ad2079096 22 extern bool mkOn;
whutsup 4:bf4ad2079096 23
whutsup 5:6bb52b2a79bf 24 #define parA 343.75f // parmeter of equation (values of distance sensor)
whutsup 5:6bb52b2a79bf 25 #define parB -1*508.08f
whutsup 5:6bb52b2a79bf 26 #define parC 166.67f
whutsup 4:bf4ad2079096 27
whutsup 4:bf4ad2079096 28
whutsup 4:bf4ad2079096 29 void MotorButton()
whutsup 4:bf4ad2079096 30 {
whutsup 4:bf4ad2079096 31
whutsup 4:bf4ad2079096 32 milLoop = 0;
whutsup 4:bf4ad2079096 33 targetDis = 0;
whutsup 4:bf4ad2079096 34 onewayNum = 0;
whutsup 4:bf4ad2079096 35 timer3.detach();
whutsup 4:bf4ad2079096 36
whutsup 4:bf4ad2079096 37 int a = up.read();
whutsup 4:bf4ad2079096 38 int b = down.read();
whutsup 4:bf4ad2079096 39 int c = a*2 + b;
whutsup 4:bf4ad2079096 40
whutsup 4:bf4ad2079096 41 switch(c)
whutsup 4:bf4ad2079096 42 {
whutsup 4:bf4ad2079096 43 case 0 : //stop
whutsup 4:bf4ad2079096 44
whutsup 4:bf4ad2079096 45 enable = 0;
whutsup 4:bf4ad2079096 46 p1 = 0;
whutsup 4:bf4ad2079096 47 p2 = 0;
whutsup 4:bf4ad2079096 48 break;
whutsup 4:bf4ad2079096 49
whutsup 4:bf4ad2079096 50 case 1 : // up
whutsup 4:bf4ad2079096 51
whutsup 4:bf4ad2079096 52 enable = 1;
whutsup 4:bf4ad2079096 53 p1 = 0;
whutsup 4:bf4ad2079096 54 p2 = 1;
whutsup 4:bf4ad2079096 55
whutsup 4:bf4ad2079096 56 break;
whutsup 4:bf4ad2079096 57
whutsup 4:bf4ad2079096 58 case 2 : // down
whutsup 4:bf4ad2079096 59
whutsup 4:bf4ad2079096 60 enable = 1;
whutsup 4:bf4ad2079096 61 p1 = 1;
whutsup 4:bf4ad2079096 62 p2 = 0;
whutsup 4:bf4ad2079096 63
whutsup 4:bf4ad2079096 64 break;
whutsup 4:bf4ad2079096 65
whutsup 4:bf4ad2079096 66 case 3 : // stop
whutsup 4:bf4ad2079096 67
whutsup 4:bf4ad2079096 68 enable = 0;
whutsup 4:bf4ad2079096 69 p1 = 0;
whutsup 4:bf4ad2079096 70 p2 = 0;
whutsup 4:bf4ad2079096 71
whutsup 4:bf4ad2079096 72 break;
whutsup 4:bf4ad2079096 73
whutsup 4:bf4ad2079096 74 }
whutsup 4:bf4ad2079096 75
whutsup 4:bf4ad2079096 76
whutsup 4:bf4ad2079096 77
whutsup 4:bf4ad2079096 78 }
whutsup 4:bf4ad2079096 79
whutsup 4:bf4ad2079096 80 void MotorTest(int mode)
whutsup 4:bf4ad2079096 81 {
whutsup 4:bf4ad2079096 82
whutsup 4:bf4ad2079096 83 timer3.detach();
whutsup 4:bf4ad2079096 84
whutsup 4:bf4ad2079096 85 switch (mode)
whutsup 4:bf4ad2079096 86
whutsup 4:bf4ad2079096 87 {
whutsup 4:bf4ad2079096 88 case 0 : //stop
whutsup 4:bf4ad2079096 89
whutsup 4:bf4ad2079096 90 enable = 0;
whutsup 4:bf4ad2079096 91
whutsup 4:bf4ad2079096 92 break;
whutsup 4:bf4ad2079096 93
whutsup 4:bf4ad2079096 94 case 1 : // up
whutsup 4:bf4ad2079096 95
whutsup 4:bf4ad2079096 96 enable = 1;
whutsup 4:bf4ad2079096 97 p1 = 0;
whutsup 4:bf4ad2079096 98 p2 = 1;
whutsup 4:bf4ad2079096 99
whutsup 4:bf4ad2079096 100 break;
whutsup 4:bf4ad2079096 101
whutsup 4:bf4ad2079096 102 case 2 : // down
whutsup 4:bf4ad2079096 103
whutsup 4:bf4ad2079096 104 enable = 1;
whutsup 4:bf4ad2079096 105 p1 = 1;
whutsup 4:bf4ad2079096 106 p2 = 0;
whutsup 4:bf4ad2079096 107
whutsup 4:bf4ad2079096 108 break;
whutsup 4:bf4ad2079096 109
whutsup 4:bf4ad2079096 110 }
whutsup 4:bf4ad2079096 111
whutsup 4:bf4ad2079096 112
whutsup 4:bf4ad2079096 113 }
whutsup 4:bf4ad2079096 114
whutsup 4:bf4ad2079096 115 void ControlAng()
whutsup 4:bf4ad2079096 116 {
whutsup 4:bf4ad2079096 117
whutsup 4:bf4ad2079096 118 float d1 = disSensor.read();
whutsup 4:bf4ad2079096 119 float d2 = parA*d1*d1+parB*d1+parC;
whutsup 4:bf4ad2079096 120 float ref_d = targetDis-d2;
whutsup 4:bf4ad2079096 121
whutsup 4:bf4ad2079096 122 if(ref_d > 0.9f)
whutsup 4:bf4ad2079096 123 {
whutsup 4:bf4ad2079096 124 enable = 1;
whutsup 4:bf4ad2079096 125 p1 = 0;
whutsup 4:bf4ad2079096 126 p2 = 1;
whutsup 4:bf4ad2079096 127
whutsup 4:bf4ad2079096 128 }
whutsup 4:bf4ad2079096 129
whutsup 4:bf4ad2079096 130 else if(ref_d < -0.9f)
whutsup 4:bf4ad2079096 131 {
whutsup 4:bf4ad2079096 132
whutsup 4:bf4ad2079096 133 enable = 1;
whutsup 4:bf4ad2079096 134 p1 = 1;
whutsup 4:bf4ad2079096 135 p2 = 0;
whutsup 4:bf4ad2079096 136
whutsup 4:bf4ad2079096 137 }
whutsup 4:bf4ad2079096 138
whutsup 4:bf4ad2079096 139 else
whutsup 4:bf4ad2079096 140 {
whutsup 4:bf4ad2079096 141 enable = 0;
whutsup 4:bf4ad2079096 142 timer1.detach();
whutsup 4:bf4ad2079096 143
whutsup 4:bf4ad2079096 144 }
whutsup 4:bf4ad2079096 145 }
whutsup 4:bf4ad2079096 146
whutsup 4:bf4ad2079096 147
whutsup 4:bf4ad2079096 148 void MkAction()
whutsup 4:bf4ad2079096 149 {
whutsup 4:bf4ad2079096 150
whutsup 4:bf4ad2079096 151
whutsup 4:bf4ad2079096 152 float d1 = disSensor.read();
whutsup 4:bf4ad2079096 153 float d2 = parA*d1*d1+parB*d1+parC;
whutsup 4:bf4ad2079096 154
whutsup 4:bf4ad2079096 155 static float curTarget;
whutsup 4:bf4ad2079096 156 float s;
whutsup 4:bf4ad2079096 157 float abs_error;
whutsup 4:bf4ad2079096 158
whutsup 5:6bb52b2a79bf 159 curTarget = (onewayNum%2==0)? targetDis: 0.0f ; // initial angle
whutsup 4:bf4ad2079096 160
whutsup 4:bf4ad2079096 161 s = (curTarget-d2)/targetDis;
whutsup 4:bf4ad2079096 162 abs_error = abs(curTarget-d2);
whutsup 4:bf4ad2079096 163
whutsup 4:bf4ad2079096 164
whutsup 4:bf4ad2079096 165 if(s>=0)
whutsup 4:bf4ad2079096 166 {
whutsup 4:bf4ad2079096 167 enable = 1;
whutsup 4:bf4ad2079096 168 p1 = 0;
whutsup 4:bf4ad2079096 169 p2 = 1; //(s>0.7f)?s:0.7f;
whutsup 4:bf4ad2079096 170 }
whutsup 4:bf4ad2079096 171
whutsup 4:bf4ad2079096 172 else
whutsup 4:bf4ad2079096 173 {
whutsup 4:bf4ad2079096 174 enable = 1;
whutsup 4:bf4ad2079096 175 p1 = 1; //(s<-0.7f)?(-1*s):0.7f;
whutsup 4:bf4ad2079096 176 p2 = 0;
whutsup 4:bf4ad2079096 177 }
whutsup 4:bf4ad2079096 178
whutsup 5:6bb52b2a79bf 179 if (abs_error <0.7f)
whutsup 4:bf4ad2079096 180 {
whutsup 4:bf4ad2079096 181
whutsup 4:bf4ad2079096 182 onewayNum = onewayNum +1;
whutsup 4:bf4ad2079096 183
whutsup 4:bf4ad2079096 184 if(onewayNum >=milLoop*2)
whutsup 4:bf4ad2079096 185 {
whutsup 4:bf4ad2079096 186 onewayNum = 0;
whutsup 4:bf4ad2079096 187 milLoop = 0;
whutsup 4:bf4ad2079096 188 enable = 0;
whutsup 4:bf4ad2079096 189 timer3.detach();
whutsup 4:bf4ad2079096 190 }
whutsup 4:bf4ad2079096 191 }
whutsup 4:bf4ad2079096 192
whutsup 4:bf4ad2079096 193
whutsup 4:bf4ad2079096 194 }
whutsup 4:bf4ad2079096 195
whutsup 4:bf4ad2079096 196 void MkActionManual()
whutsup 4:bf4ad2079096 197 {
whutsup 4:bf4ad2079096 198
whutsup 4:bf4ad2079096 199 float d1 = disSensor.read();
whutsup 4:bf4ad2079096 200 float d2 = parA*d1*d1+parB*d1+parC;
whutsup 4:bf4ad2079096 201
whutsup 4:bf4ad2079096 202 enable = 0;
whutsup 4:bf4ad2079096 203 timer3.detach();
whutsup 4:bf4ad2079096 204 onewayNum=0;
whutsup 4:bf4ad2079096 205 targetDis = d2;
whutsup 4:bf4ad2079096 206 milLoop = 99;
whutsup 4:bf4ad2079096 207 timer3.attach(&MkAction,0.1);
whutsup 4:bf4ad2079096 208
whutsup 4:bf4ad2079096 209 }