CatPot 2015-2016
2
Dependencies: AQM0802A HMC6352 PID Servo mbed
Diff: main.cpp
- Revision:
- 5:dace4f3b6e4a
- Parent:
- 4:f7946508daa8
--- 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;
+
}
/*