DECS @UNIST
/
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
Diff: MotorControl.cpp
- Revision:
- 19:67584cb64b9c
- Parent:
- 15:e5e34512a00e
--- a/MotorControl.cpp Thu Jul 04 08:08:18 2019 +0000
+++ b/MotorControl.cpp Thu Aug 22 08:36:15 2019 +0000
@@ -1,28 +1,22 @@
- #include "mbed.h"
+#include "mbed.h"
-extern Serial bt;
-extern Serial pc;
+extern Serial bt;
+//extern Serial pc;
+
-//---------------------- testing ------------- 190703 BH
-//DigitalOut enable(PB_12);
-//DigitalOut nreset(PC_6);
-//PwmOut p1(PB_13);
-//PwmOut p2(PB_14);
-// -> changed like below
-extern DigitalOut enable;
-extern DigitalOut nreset;
-extern PwmOut p1;
-extern PwmOut p2;
-//---------------------- end ------------- 190703 BH
+extern DigitalOut enable;
+extern DigitalOut nreset;
+extern PwmOut p1;
+extern PwmOut p2;
-extern InterruptIn up;
-extern InterruptIn down;
+extern InterruptIn up;
+extern InterruptIn down;
-extern AnalogIn disSensor;
+extern AnalogIn disSensor;
-extern Serial bt;
-extern Ticker timer1;
-extern Ticker timer3;
+extern Serial bt;
+extern Ticker timer1;
+extern Ticker timer3;
extern int targetDis;
extern int milLoop;
@@ -30,24 +24,35 @@
extern bool mkOn;
extern float errorPrevious;
-extern float parA;
-extern float parB;
-extern float parC;
+extern float parA;
+extern float parB;
+extern float parC;
+
+extern bool nFaultFlag;
+
+#ifdef QUALIFYING_MODE
-extern bool nFaultFlag;
+extern Timer Qtimer; // timer for qualifying test
+extern int Qtime; // Qtime stores result from Qtimer
+extern bool Qflag; // Qflag rises when result is stored
+void Qfunc(){
+ Qtimer.stop();
+ Qtime = Qtimer.read_us();
+ Qflag = true;
+}
+#endif
void MotorButton()
{
mkOn=0;
milLoop = 0;
-// targetDis = 0;
onewayNum = 0;
timer3.detach();
float d1 = disSensor.read();
float d2 = parA*d1*d1+parB*d1+parC;
- targetDis = d2; // setup to target distance in MkActionManual
+ targetDis = d2; // setup to target distance in MkActionManual
int a = up.read();
int b = down.read();
@@ -55,42 +60,39 @@
switch(c)
{
-
-
case 0 : //stop
enable = 0;
nreset = 0;
p1 = 0;
p2 = 0;
-
+ #ifdef QUALIFYING_MODE
+ Qfunc();
+ #endif
break;
case 1 : // up
- {
-
+
enable = 1;
nreset = 1;
p1 = 0;
p2 = 1;
-
-
+ #ifdef QUALIFYING_MODE
+ Qfunc();
+ #endif
break;
- }
case 2 : // down
- {
-
-
-
+
enable = 1;
nreset = 1;
p1 = 1;
p2 = 0;
-
-
+ #ifdef QUALIFYING_MODE
+ Qfunc();
+ #endif
break;
- }
+
case 3 : // stop
@@ -98,7 +100,9 @@
nreset = 0;
p1 = 0;
p2 = 0;
-
+ #ifdef QUALIFYING_MODE
+ Qfunc();
+ #endif
break;
}
@@ -153,7 +157,6 @@
float d1 = disSensor.read();
float d2 = parA*d1*d1+parB*d1+parC;
float ref_d = targetDis-d2;
-// float kp = ref_d/(errorPrevious);
if(ref_d > 0.3f)
{
@@ -161,19 +164,13 @@
nreset = 1;
p1 = 0;
p2 = 1;
-// p2 = (kp<0.4?0.4:kp);
-// p2 = (ref_d>1?1:ref_d);
-
}
else if(ref_d < -0.3f)
{
-
enable = 1;
nreset = 1;
-// p1 = -1*(ref_d<-1?-1:ref_d);
p1 = 1;
-// p1 = -1*(kp>0.4?0.4:kp);
p2 = 0;
}
@@ -182,15 +179,12 @@
enable = 0;
nreset = 0;
timer1.detach();
-
}
}
void MkAction()
{
-// bt.putc('3');
-
float d1 = disSensor.read();
float d2 = parA*d1*d1+parB*d1+parC;
@@ -203,15 +197,11 @@
s = (curTarget-d2)/targetDis;
abs_error = abs(curTarget-d2);
- bt.printf("%dth: %1.4f\r\n",onewayNum,s);
-
- if(!nFaultFlag)
+ if(!nFaultFlag)
{
+ p1 = 0;
+ p2 = 0;
return;
-// enable = 0;
-// nreset = 0;
-// p1=0;
-// p2=0;
}
if(s>=0)
{
@@ -228,14 +218,12 @@
p2 = 0;
p1 = 1;
}
-
-
if (abs_error <1.5f)
{
onewayNum = onewayNum +1;
enable = 0;
- nreset = 0; //Hbridge ouput reset! 190627 nreset problem
+ nreset = 0;
if(onewayNum >=milLoop*2)
{
@@ -246,24 +234,13 @@
timer3.detach();
bt.puts("<ME>\r\n");
-
-// bt.putc('<');
-// bt.putc('M');
-// bt.putc('E');
-// bt.putc('>');
-// bt.putc('\r');
-// bt.putc('\n');
}
}
-
-
}
void MkActionManual()
{
-
-
- if( 1 == mkOn)
+ if( 1 == mkOn)
{
mkOn = 0;
enable = 0;
@@ -274,10 +251,8 @@
else
{
mkOn = 1;
- onewayNum=0;
+ onewayNum=0;
milLoop = 99;
timer3.attach(&MkAction,0.1);
}
-
-
}
\ No newline at end of file