CatPot 2015-2016
2
Dependencies: AQM0802A HMC6352 PID Servo mbed
Revision 5:dace4f3b6e4a, committed 2015-03-04
- Comitter:
- ryuna
- Date:
- Wed Mar 04 07:01:19 2015 +0000
- Parent:
- 4:f7946508daa8
- Commit message:
- add one function _fool
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
| main.h | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Tue Mar 03 02:36:28 2015 +0000
+++ b/main.cpp Wed Mar 04 07:01:19 2015 +0000
@@ -53,12 +53,13 @@
Led[0] = 0;
Led[1] = !val;
- //wait_ms(.25);
+ wait_ms(1);
val = Sensor.read(Address|1, output, num);// IRデータを受信
Led[0] = !val;
Led[1] = 0;
+ wait_ms(1);
}
void ReceiveFast(uint8_t Address, char kind, char output[],int num){
@@ -75,9 +76,10 @@
**/
bool val;
+ Led[0] = 0;
val = Sensor.read(Address|1,output,num);
+ wait_ms(1);
Led[0] = !val;
- Led[1] = 0;
}
@@ -90,9 +92,8 @@
*/
char data_r[3] = {0},data_l[3] = {0};
- ReceiveFast(ADDRESS_R,BALL,data_r,3);
- wait_ms(0.5);
- ReceiveFast(ADDRESS_L,BALL,data_l,3);
+ Receive(ADDRESS_R,BALL,data_r,3);
+ Receive(ADDRESS_L,BALL,data_l,3);
if((data_r[0] == 125)||(data_l[0] == 125)){/*ボールを検知しているかチェック*/
if((data_r[0] == 125)&&(data_l[0] == 125)){
@@ -143,7 +144,6 @@
char data_r[3] = {0},data_l[3] = {0};
ReceiveFast(ADDRESS_R,BALL,data_r,3);
- wait_ms(0.5);
ReceiveFast(ADDRESS_L,BALL,data_l,3);
if((data_r[0] == 125)||(data_l[0] == 125)){/*ボールを検知しているかチェック*/
@@ -191,10 +191,10 @@
}
-void move(int vr,int vl, double vs ){
+void move(int vr,int vl, double vs ,int Rad){
double pwm[4] = {0};
uint8_t i = 0;
- pwm[0] = -vr + vs;
+ pwm[0] = vr - vs;
pwm[1] = 0;
pwm[2] = 0;
pwm[3] = vl + vs;
@@ -207,69 +207,63 @@
}
speed[i] = pwm[i];
}
+ SetRad = Rad;
}
+void fool (int *Rad, int *Power){
+ static int Last_Rad = 0;
+ static int Last_Vector = 1;
+ int rad = *Rad;
+ int Temp;
+ Temp = Last_Rad % 180;
-char LazyBum(unsigned int Rad){//怠け者
-/*
- * Radは角度(0~360),
- * Lastには過去の角度(0~360)
- *
- * 最終的には+-90度で表すことになる．
- */
-/*
- static unsigned int Last;//static付けて宣言すると勝手に0で初期化されるハズ....
- static char Rotation = CW;//CW or CCW
- char trans[4] = {1,-1,1,-1};
- unsigned int temp,temp0 ;
-
- temp = Rad%180;
-
- if((temp > 70)&&(temp < 110)){
- temp0 = Last%180;
-
- if((temp0 > 70)&&(temp0 < 110)){
-
- if(abs(Rad - Last)<40){{
-
- if(Rad<110){
- S555.position(Rad*1.0);
- Last = Rad;
- Rotation = trans[Rad/110];
- return;
+ if((Temp>70) &&(Temp<110)){
+ Temp = *Rad % 180;
+ if((Temp>70) &&(Temp<110)){
+ Temp = abs(*Rad - Last_Rad);
+ if(Temp>160){
+ Last_Vector = -1 * Last_Vector;//正転逆転切り替え
+ if(*Rad/180){
+ *Rad = Angle[Last_Rad%180] -(Last_Rad - *Rad%180);
+ *Power = *Power * Last_Vector;
+ }else{
+ *Rad = Angle[Last_Rad%180] -(Last_Rad%180 - *Rad);
+ *Power = *Power * Last_Vector;
}
- S555.position((-360+Rad)*1.0);
- Last = Rad;
- Rotation = 1;
- return Rotation;
- }
- if(Last<110){
- S555.postiton((-360+Rad)*-1.0);
- Last = Rad;
- Rotation = trans[(90+Rad)/90];
- return Rotation;
+ Last_Rad = rad;
+ return;
+ }else if((Last_Vector+2) == 1){
+ /*逆転のまま角度拡張*/
+ if(*Rad/180){
+ *Rad = -360 + *Rad ;
+ *Power = *Power * Last_Vector;
+ }else{
+ *Power = *Power * Last_Vector;
+ }
+ Last_Rad = rad;
+ return;
+
+ }else if((Last_Vector+2) == 3){
+ /*正転のまま*/
+ if(*Rad/180){
+ *Rad = -360 + *Rad ;
+ *Power = *Power * Last_Vector;
+ }else{
+ *Power = *Power * Last_Vector;
+ Last_Rad = rad;
+ }
+ Last_Rad = rad;
+ return;
}
- S555.postiton(Rad*-1.0);
- Last = Rad;
- Rotation = trans[(90+Rad)/90];
- return Rotation;
}
}
-
- if(temp < 90){
- S555.position((Rad%90)*1.0);
- Rotation = [Rad/90];
- Last = Rad;
- return Rotation;
- }
- S555.position((Rad%90- 180)*1.0);
- Last = Rad;
- Rotation = [Rad/90];
- return Rotation;
- */
- return 0;
+ /*通常動作*/
+ Last_Vector = RadToVector[(*Rad-1)/90];
+ *Rad = Angle[*Rad%180];
+ *Power = *Power * Last_Vector;
+ Last_Rad = rad;
+
}
-
void IrFrontAction(int *CompassDef , uint8_t IrMemo[],double ReV)//ball 12 or 0 o-clock
{
@@ -305,7 +299,7 @@
{
//止まっとく
S555.position(0.0);
- move(0,0,ReV);
+ move(0,0,ReV,0);
/*line検知をしないバージョン*/
/*
@@ -323,18 +317,18 @@
*/
}
-double PidUpdate(int CompassDef , double SetAngle)
+void PidUpdate(int CompassDef , double SetAngle, double *Pid_up)
{
int Compass;
double inputPID = 0.0;
Compass = ((hmc6352.sample() / 10) + 540 - CompassDef) % 360;
-
+ wait_ms(10);
pid.setSetPoint((int)((REFERENCE + SetAngle) / 1.0));
inputPID = Compass;
pid.setProcessValue(inputPID);
- return -(pid.compute());
+ *Pid_up = -(pid.compute());
}
uint8_t SwRead(){
@@ -371,6 +365,7 @@
void tx_motor(){
array(speed[0],speed[1],speed[3],speed[2]);
Motor.printf("%s",StringFIN.c_str());
+ S555.position(SetRad);
}
void SetUp(int *CompassDef){
@@ -389,15 +384,16 @@
Motor.baud(115200); //ボーレート設定
Motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
Motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用）
- //move(0,0,0);//停止
+
S555.calibrate(0.0005, 120.0);
S555.position(0.0); //初期位置にセット
-
+ move(0,0,0,0);//停止
+ Sensor.frequency(100000);
hmc6352.setOpMode(HMC6352_CONTINUOUS, 1, 20);
*CompassDef = (hmc6352.sample() / 10);
- for(i = 0;i<10;i++){
- ReV = PidUpdate(*CompassDef,0);
+ for(i = 0;i<15;i++){
+ PidUpdate(*CompassDef,0,&ReV);
}
Lcd.printf("%f\n",ReV);
@@ -509,6 +505,14 @@
}
if(State == Calibration){
Led[0] = Led[1] = Led[2] = 0;
+ move(20,20,0,0);
+ while((State == Calibration)){
+ wait_ms(100);
+ State = SwRead();
+
+ }
+ move(0,0,0,0);
+ /*
hmc6352.setCalibrationMode(ENTER);
while((State == Calibration)){
State = SwRead();
@@ -516,7 +520,7 @@
hmc6352.setCalibrationMode(EXIT);
wait(0.3);//必要
Led[3] = 0;
-
+ */
/*calibration command enter*/
continue;
}
@@ -540,6 +544,7 @@
/*PID補正move加算値 Revise */
double ReV = 0.0;
+ int Rad = 0;
/*State */
//Direction LineIr = NA;//方位設定奴...エラーでてめんどくさいので後でまたやることにする。
@@ -547,7 +552,7 @@
uint8_t IrChange[13] ={0x01,0x01,0x02,0x02,0x02,0x04,
0x04,0x04,0x08,0x08,0x08,0x01,0x00};
-
+ int Power = 0;
/*関数ポインタ*/
void (*AnotherAction[3])(int *, uint8_t [],double);
@@ -571,32 +576,43 @@
Led[0] = 1;
/*白線を読んでいないか確認する*/
- /*
+
LineData = (~Line+0x00) & 0x0F;
if(LineData){
- IrNum = IrReceiveFast();
- LineIr = LineData & IrChange[IrNum]; //一箇所でも一致すればlineを検知している．
- while(LineIr){
- move(0,0,0);//
+ //IrNum = IrReceiveFast();
+ //LineIr = LineData & IrChange[IrNum]; //一箇所でも一致すればlineを検知している．
+ while(LineData){
+ move(0,0,0,0);//
Led[1] = Led[2] = Led[3] = 1;
LineData = (~Line+0x00) & 0x0F;
- IrNum = IrReceiveFast();
- LineIr = LineData & IrChange[IrNum];//一箇所でも一致すればlineを検知している．
+ //IrNum = IrReceiveFast();
+ wait_ms(10);
+ //LineIr = LineData & IrChange[IrNum];//一箇所でも一致すればlineを検知している．
+
}
Led[1] = Led[2] = Led[3] = 0;
- continue;
+ //wait(0.02);
+
}
- */
+
+ Led[2] = 1;
IrNum = IrReceive(IrData);
- ReV = 0;//PidUpdate(CompassDef ,SetAngle);
- move(vr[IrNum],vl[IrNum],ReV);
- wait_ms(10);
- S555.position(s_deg[IrNum]);
+ wait_ms(100);
+ Led[2] = 0;
+ //PidUpdate(CompassDef ,SetAngle,&ReV);
+ Led[3] = 1;
+ Power = 20;
+ Rad = 359-IrNum*30;
+ fool(&Rad,&Power);
+ move(Power,Power,ReV,Rad);
+ wait_ms(100);
+ Led[3] = 0;
-
+ Led[2] = 1;
+ wait_ms(200);
- Led[0] = 0;
- wait_ms(200);
+ Led[0] = Led[1] = Led[2] = Led[3] = 0;
+
}
/*
--- a/main.h Tue Mar 03 02:36:28 2015 +0000
+++ b/main.h Wed Mar 04 07:01:19 2015 +0000
@@ -55,7 +55,7 @@
int speed[4] = {0};
typedef enum {FRONT = 1, RIGHT, BACK, LEFT, NA} Direction;
enum {SONIC = 0x01, BALL};
-
+int SetRad = 0;
int TargetCoordinates[12][2][2] = {
{{0,1},{0,1}},//p,q+6
@@ -72,6 +72,56 @@
{{0,1},{0,1}},//0,q
};
+volatile int Angle[180] = {
+ 0 , 1 , 2 , 3 ,
+ 4 , 5 , 6 , 7 ,
+ 8 , 9 , 10 , 11 ,
+ 12 , 13 , 14 , 15 ,
+ 16 , 17 , 18 , 19 ,
+ 20 , 21 , 22 , 23 ,
+ 24 , 25 , 26 , 27 ,
+ 28 , 29 , 30 , 31 ,
+ 32 , 33 , 34 , 35 ,
+ 36 , 37 , 38 , 39 ,
+ 40 , 41 , 42 , 43 ,
+ 44 , 45 , 46 , 47 ,
+ 48 , 49 , 50 , 51 ,
+ 52 , 53 , 54 , 55 ,
+ 56 , 57 , 58 , 59 ,
+ 60 , 61 , 62 , 63 ,
+ 64 , 65 , 66 , 67 ,
+ 68 , 69 , 70 , 71 ,
+ 72 , 73 , 74 , 75 ,
+ 76 , 77 , 78 , 79 ,
+ 80 , 81 , 82 , 83 ,
+ 84 , 85 , 86 , 87 ,
+ 88 , 89 , 90 , -89 ,
+ -88 , -87 , -86 , -85 ,
+ -84 , -83 , -82 , -81 ,
+ -80 , -79 , -78 , -77 ,
+ -76 , -75 , -74 , -73 ,
+ -72 , -71 , -70 , -69 ,
+ -68 , -67 , -66 , -65 ,
+ -64 , -63 , -62 , -61 ,
+ -60 , -59 , -58 , -57 ,
+ -56 , -55 , -54 , -53 ,
+ -52 , -51 , -50 , -49 ,
+ -48 , -47 , -46 , -45 ,
+ -44 , -43 , -42 , -41 ,
+ -40 , -39 , -38 , -37 ,
+ -36 , -35 , -34 , -33 ,
+ -32 , -31 , -30 , -29 ,
+ -28 , -27 , -26 , -25 ,
+ -24 , -23 , -22 , -21 ,
+ -20 , -19 , -18 , -17 ,
+ -16 , -15 , -14 , -13 ,
+ -12 , -11 , -10 , -9 ,
+ -8 , -7 , -6 , -5 ,
+ -4 , -3 , -2 , -1 };
+
+volatile int RadToVector[4] = {1,-1,-1,1};
+
+
/*
| |
| |