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
Revision 19:67584cb64b9c, committed 2019-08-22
- Comitter:
- Bhoney
- Date:
- Thu Aug 22 08:36:15 2019 +0000
- Parent:
- 18:25281ee3a517
- Commit message:
- QUALIFYING_MODE is added (Qtimer, Qtime, Qflag, Qfunc for btn)
Changed in this revision
--- a/ForceRead.cpp Thu Jul 04 08:08:18 2019 +0000
+++ b/ForceRead.cpp Thu Aug 22 08:36:15 2019 +0000
@@ -1,18 +1,12 @@
#include "mbed.h"
-AnalogIn force_L1(PB_0); // Force Sensor
-AnalogIn force_L2(PB_1);
-AnalogIn force_L3(PC_0);
-AnalogIn force_L4(PC_1);
AnalogIn force_R1(PC_2);
AnalogIn force_R2(PC_3);
AnalogIn force_R3(PC_4);
AnalogIn force_R4(PC_5);
extern Serial bt;
-extern Serial pc;
-
-char readCount=0;
+//extern Serial pc;
float sum_L[4]={0,};
float sum_R[4]={0,};
@@ -20,120 +14,11 @@
float avg_L[4]={0,};
float avg_R[4]={0,};
-
-/*----------------------- Callback Functions ------------------------*/
-
-void CalForce()
+void ReadForce()
{
- float force_L[4];
- float force_R[4];
-
- force_L[0] = force_L1.read();
- force_L[1] = force_L2.read();
- force_L[2] = force_L3.read();
- force_L[3] = force_L4.read();
- force_R[0] = force_R1.read();
- force_R[1] = force_R2.read();
- force_R[2] = force_R3.read();
- force_R[3] = force_R4.read();
-
-
- if ( readCount < 6)
- {
- sum_L[0] = sum_L[0] + force_L[0];
- sum_L[1] = sum_L[1] + force_L[1];
- sum_L[2] = sum_L[2] + force_L[2];
- sum_L[3] = sum_L[3] + force_L[3];
-
- sum_R[0] = sum_R[0] + force_R[0];
- sum_R[1] = sum_R[1] + force_R[1];
- sum_R[2] = sum_R[2] + force_R[2];
- sum_R[3] = sum_R[3] + force_R[3];
-
- readCount++;
-
- }
-
- else if (readCount == 6 | readCount>6)
- {
-
- avg_L[0] = sum_L[0]/6;
- avg_L[1] = sum_L[1]/6;
- avg_L[2] = sum_L[2]/6;
- avg_L[3] = sum_L[3]/6;
-
- avg_R[0] = sum_R[0]/6;
- avg_R[1] = sum_R[1]/6;
- avg_R[2] = sum_R[2]/6;
- avg_R[3] = sum_R[3]/6;
-
-// bt.printf("%1.3f,%1.3f,%1.3f,%1.3f,%1.3f,%1.3f,%1.3f,%1.3f\n", avg_L[0],avg_L[1],avg_L[2],avg_L[3],avg_R[0],avg_R[1],avg_R[2],avg_R[3]);
- bt.putc('<');
- bt.putc('F');
- bt.putc('O');
- bt.putc('T');
- bt.putc((unsigned char)((int)avg_L[0]*127));
- bt.putc((unsigned char)((int)avg_L[1]*127));
- bt.putc((unsigned char)((int)avg_L[2]*127));
- bt.putc((unsigned char)((int)avg_L[3]*127));
- bt.putc((unsigned char)((int)avg_R[0]*127));
- bt.putc((unsigned char)((int)avg_R[1]*127));
- bt.putc((unsigned char)((int)avg_R[2]*127));
- bt.putc((unsigned char)((int)avg_R[3]*127));
- bt.putc('>');
- bt.putc('\r');
- bt.putc('\n');
+ int force_R[4];
- readCount=0;
-
- for(int i =0; i<4 ; i++)
- {
- sum_L[i] = 0;
- sum_R[i] = 0;
- avg_L[i] = 0;
- avg_R[i] = 0;
- }
-
-
- }
-
-
-}
-
-void ReadForce()
-{
-// float force_L[4]; // not used
-// force_L[0] = force_L1.read(); // not used
-// force_L[1] = force_L2.read(); // not used
-// force_L[2] = force_L3.read(); // not used
-// force_L[3] = force_L4.read(); // not used
-
-
-
-// bt.printf("%1.3f,%1.3f,%1.3f,%1.3f\n",force_R[0],force_R[1],force_R[2],force_R[3]); //debug purpose
-// ======================================== BH 190704 reomoved
-// float force_R[4];
-// force_R[0] = force_R1.read();
-// force_R[1] = force_R2.read();
-// force_R[2] = force_R3.read();
-// force_R[3] = force_R4.read();
-
-// bt.putc('<');
-// bt.putc('F');
-// bt.putc('O');
-// bt.putc('T');
-//
-// bt.putc((char)((unsigned int)(force_R[0]*127)));
-// bt.putc((char)((unsigned int)(force_R[1]*127)));
-// bt.putc((char)((unsigned int)(force_R[2]*127)));
-// bt.putc((char)((unsigned int)(force_R[3]*127)));
-// bt.putc('>');
-// bt.putc('\r');
-// bt.putc('\n');
-// ======================================== BH 190704 added
- int force_R[4];
-// force_R[i]*=100; //forcr_R range = 0~1 -> changed 0~99
force_R[0] = force_R1.read()*99;
force_R[1] = force_R2.read()*99;
force_R[2] = force_R3.read()*99;
@@ -143,5 +28,4 @@
force_R[1],
force_R[2],
force_R[3]);
-
}
\ No newline at end of file
--- a/ForceRead.h Thu Jul 04 08:08:18 2019 +0000 +++ b/ForceRead.h Thu Aug 22 08:36:15 2019 +0000 @@ -1,2 +1,6 @@ +#ifndef FORCE_READ_H +#define FORCE_READ_H + void ReadForce(); -void CalForce(); \ No newline at end of file + +#endif \ No newline at end of file
--- 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
--- a/MotorControl.h Thu Jul 04 08:08:18 2019 +0000 +++ b/MotorControl.h Thu Aug 22 08:36:15 2019 +0000 @@ -1,6 +1,11 @@ +#ifndef MOTOR_CONTROL_H +#define MOTOR_CONTROL_H + void MotorButton(); void ControlAng(); void MotorTest(int mode); void ButtonOn(); void MkAction(); -void MkActionManual(); \ No newline at end of file +void MkActionManual(); + +#endif \ No newline at end of file
--- a/main.cpp Thu Jul 04 08:08:18 2019 +0000
+++ b/main.cpp Thu Aug 22 08:36:15 2019 +0000
@@ -2,13 +2,16 @@
#include "MotorControl.h"
#include "ForceRead.h"
-Serial bt(PA_9,PA_10); // USART1 Bluetooth (Tx, Rx)
-//Serial pc(PA_2,PA_3); // USART2 pc (Tx, Rx)
-Serial pc(USBTX,USBRX); // USART2 pc (Tx, Rx)
+#define QUALIFYING_MODE
+
+Serial bt(PA_2,PA_3); // USART1 Bluetooth (Tx, Rx)
+// Serial pc(PA_2,PA_3); // USART2 pc (Tx, Rx)
+//Serial pc(USBTX,USBRX); // USART2 pc (Tx, Rx)
InterruptIn up(PC_8); // Button Interrupt Motor control
InterruptIn down(PC_9);
InterruptIn bt_ml(PC_10); // Millking action button
+InterruptIn nFault(PB_15); // Motor Driver DRV
DigitalOut led(PA_5); // Operating LED signal
@@ -16,30 +19,159 @@
Timeout ResetTimer; // Reset angle 0 position
+#ifdef QUALIFYING_MODE
+Timer Qtimer; // timer for qualifying test
+int Qtime; // Qtime stores result from Qtimer
+bool Qflag; // Qflag rises when result is stored
+#endif
+DigitalOut enable(PB_12);
+DigitalOut nreset(PC_6);
+PwmOut p1(PB_13);
+PwmOut p2(PB_14);
+
+
Ticker timer0;
Ticker timer1; // Control Angle By Using Distance Sensor
Ticker timer2; // Force Sensor
Ticker timer3; // Millking Action Mode
//Ticker timer4;
+
+/*----------------------- Global Variables ------------------------*/
+
+char rxData[7];
+bool flagRx = 0;
+
int targetDis; // Target Of Distance
int milLoop; // Number of Millking Action
-//float errorPrevious;
int onewayNum=0; // Counting Turning Point
int sysStatus=0;
bool mkOn = 0;
+bool nFaultFlag = true;
-/*----------------------- ReadData Variables ------------------------*/
+float parA = 73.671f;
+float parB = -1*206.25f;
+float parC = 106.43f;
+/*----------------------- Callback Function Prototypes ------------------------*/
+void nFaultLow();
+void nFaultHigh();
+void ButtonSys();
+void ManualMk();
+void OperateLED();
+void ModeSelect(int mode);
+void ReadAng();
+void stop();
+void ReadData();
+void Test();
+/*----------------------- Main function ------------------------*/
+int main()
+{
+ bt.baud(115200);
+// pc.baud(115200);
+
+
+ #ifdef QUALIFYING_MODE
+ bt.puts("START 190822 Q_MODE\r\n");
+ #else
+ bt.puts("START 190822 Ver.9\r\n");
+ #endif
+
+// pc.puts("START 190703 Ver.9\r\n");
+ int modeNum = 0;
+ int modeMotor = 0;
+
+ char tmpCommand[4]={0,}; // [ADD] command
+ int rxVal;
+
+ nFault.mode(PullUp);
+ nFault.fall(&nFaultLow);
+ nFault.rise(&nFaultHigh);
+ up.mode(PullNone);
+ down.mode(PullNone);
+ bt_ml.mode(PullNone); // [ADD] Setup Millking Action Manual Button
+ up.fall(&ButtonSys);
+ up.rise(&ButtonSys);
+ down.fall(&ButtonSys);
+ down.rise(&ButtonSys);
+
+ bt_ml.fall(&ManualMk); // [ADD] Interrupt Millking Action Manual Button
+ timer0.attach(&OperateLED,0.2);
+
+// timer4.attach(&Test,0.1);
+
+ bt.attach(&ReadData, Serial::RxIrq);
+
-char rxData[7];
-bool flagRx = 0;
+ while(1)
+ {
+ #ifdef QUALIFYING_MODE
+ if(Qflag){
+ Qflag = false;
+ bt.printf("%dus from btn to motor output\r\n",Qtime);
+ }
+ #endif
+
+ if(!nFaultFlag)
+ {
+ enable = 0;
+ nreset = 0;
+ p1=0;
+ p2=0;
+ wait(0.01f);
+ }
+
+ if (1 == flagRx)
+ {
+ flagRx = 0;
+ tmpCommand[0] = rxData[0];
+ tmpCommand[1] = rxData[1];
+ tmpCommand[2] = rxData[2];
+ rxVal = atoi(rxData+3);
+
+ if (0 == strcmp(tmpCommand,"MOD"))
+ {
+ bt.puts("<MOD>\r\n");
+ modeNum = rxVal; // 1 ~ 6
+ sysStatus = rxVal; // Why this value are assigned directly?
+ // assigning sys variable after checking condition is correct progress, i think.
+ ModeSelect(modeNum);
+ }
+
+ else if (0 == strcmp(tmpCommand,"MOT"))
+ {
+ bt.puts("<MOT>\r\n");
+ modeMotor = rxVal;
+ MotorTest(modeMotor);
+ }
+
+ else if (0 == strcmp(tmpCommand,"POS"))
+ {
+ bt.puts("<POS>\r\n");
+ targetDis = rxVal;
+ timer1.attach(&ControlAng,0.01);
+ }
+
+ if (0 == strcmp(tmpCommand,"MIL"))
+ {
+ bt.puts("<MIL>\r\n");
+ sysStatus = 5;
+ targetDis = atoi(rxData+5); // [ADD] return 2 characters in the back of 4 characters
+ milLoop = 0.01*(atoi(rxData+3)-atoi(rxData+5)); // [ADD] return 2 cahracters in the front of 4 characters
+ timer3.attach(&MkAction,0.1);
+
+ if ( 0 == mkOn) mkOn = 1;
+
+ }
+ }
+
+
+ }
+
+}
-float parA =73.671f;
-float parB =-1*206.25f;
-float parC =106.43f;
/*----------------------- Callback Functions ------------------------*/
@@ -90,7 +222,6 @@
}
}
-
void stop()
{
MotorTest(0);
@@ -101,12 +232,7 @@
float d1 = disSensor.read();
float d2 = parA*d1*d1+parB*d1+parC;
-// original
-// bt.printf("%1.2f",d2);
-
-// fixed
bt.printf("<DIO%1.2f>",d2);
-
}
void ModeSelect(int mode)
@@ -115,12 +241,10 @@
switch(mode)
{
- case 0 : //Standard Mode
-
- timer1.detach();
- timer2.detach();
- timer3.detach();
-
+ case 0 : //Standard Mode
+ timer1.detach();
+ timer2.detach();
+ timer3.detach();
break;
case 1 : //Reset Mode
@@ -136,7 +260,7 @@
MotorTest(2);
ResetTimer.attach(&stop,0.5f+d2/3.0f);
-
+
break;
}
@@ -163,7 +287,6 @@
}
}
-
void OperateLED()
{
led =!led;
@@ -171,7 +294,6 @@
void ManualMk()
{
-
if( 1 == sysStatus | 2 == sysStatus)
{
bt.puts("Please Wait Moving Anybaro"); //<STA>
@@ -181,12 +303,15 @@
sysStatus = 6;
MkActionManual();
}
-
-
}
void ButtonSys()
{
+ #ifdef QUALIFYING_MODE
+ Qtimer.reset();
+ Qtimer.start();
+ Qflag = false;
+ #endif
if( 1 == sysStatus | 2 == sysStatus)
{
bt.puts("Please Wait Moving Anybaro"); //<STA> 또는 ifdef debug 처리?
@@ -197,11 +322,7 @@
MotorButton();
}
}
-//---------------------- testing ------------- 190703 BH
-InterruptIn nFault(PB_15); // Motor Driver DRV
-bool nFaultFlag = true;
-//this interrupt function might not be necessary. need to be tested.
void nFaultHigh(){
nFaultFlag = true;
}
@@ -210,117 +331,5 @@
nFaultFlag = false;
}
-DigitalOut enable(PB_12);
-DigitalOut nreset(PC_6);
-PwmOut p1(PB_13);
-PwmOut p2(PB_14);
-
-//---------------------- end ------------- 190703 BH
-int main()
-{
- bt.baud(115200);
- pc.baud(115200);
- bt.puts("START 190703 Ver.8 \r\n");
- pc.puts("START 190703 Ver.8 \r\n");
- int modeNum = 0;
- int modeMotor = 0;
-
- char tmpCommand[4]={0,}; // [ADD] command
- int rxVal;
-
- //---------------------- testing ------------- 190703 BH
- nFault.mode(PullUp);
- nFault.fall(&nFaultLow);
-
- //this interrupt function might not be necessary. need to be tested.
- nFault.rise(&nFaultHigh);
-
- //---------------------- end ------------- 190703 BH
-
- up.mode(PullNone);
- down.mode(PullNone);
- bt_ml.mode(PullNone); // [ADD] Setup Millking Action Manual Button
-
- up.fall(&ButtonSys);
- up.rise(&ButtonSys);
- down.fall(&ButtonSys);
- down.rise(&ButtonSys);
-
- bt_ml.fall(&ManualMk); // [ADD] Interrupt Millking Action Manual Button
-
- timer0.attach(&OperateLED,0.2);
-// timer4.attach(&Test,0.1);
- bt.attach(&ReadData, Serial::RxIrq);
-
-
- while(1)
- {
-
- //---------------------- need to be tested ------------- BH
- if(!nFaultFlag)
- {
-
-// test below code (without rise interrupt callback)
-// nFaultFlag = true;
- enable = 0;
- nreset = 0;
- p1=0;
- p2=0;
- wait(0.01f);
- }
- //---------------------- end ------------- 190703 BH
-
- if (1 == flagRx)
- {
-
- flagRx = 0;
- tmpCommand[0] = rxData[0];
- tmpCommand[1] = rxData[1];
- tmpCommand[2] = rxData[2];
- rxVal = atoi(rxData+3);
-
-
- if (0 == strcmp(tmpCommand,"MOD"))
- {
- modeNum = rxVal; // 1 ~ 6
- sysStatus = rxVal; // Why this value are assigned directly?
- // assigning sys variable after checking condition is correct progress, i think.
- ModeSelect(modeNum);
- }
-
- else if (0 == strcmp(tmpCommand,"MOT"))
- {
- modeMotor = rxVal;
- MotorTest(modeMotor);
- }
-
- else if (0 == strcmp(tmpCommand,"POS"))
- {
-// errorPrevious = 0;
- targetDis = rxVal;
-// float d1 = disSensor.read();
-// float d2 = parA*d1*d1+parB*d1+parC;
-// errorPrevious = targetDis - d2;
-
- timer1.attach(&ControlAng,0.01);
- }
-
- if (0 == strcmp(tmpCommand,"MIL"))
- {
- sysStatus = 5;
- targetDis = atoi(rxData+5); // [ADD] return 2 characters in the back of 4 characters
- milLoop = 0.01*(atoi(rxData+3)-atoi(rxData+5)); // [ADD] return 2 cahracters in the front of 4 characters
- timer3.attach(&MkAction,0.1);
-
- if ( 0 == mkOn) mkOn = 1;
-
- }
-
-
- }
-
-
- }
-
-}
\ No newline at end of file
+//---------------------- end -------------
\ No newline at end of file
--- a/mbed.bld Thu Jul 04 08:08:18 2019 +0000 +++ b/mbed.bld Thu Aug 22 08:36:15 2019 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/mbed_official/code/mbed/builds/5aab5a7997ee \ No newline at end of file +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file