CatPot 2015-2016
version 3 通信方式，マイコン等に変更あり
Dependencies: AQM0802A PID Servo mbed
Revision 4:2857f273a7f4, committed 2015-03-11
- Comitter:
- ryuna
- Date:
- Wed Mar 11 01:03:00 2015 +0000
- Parent:
- 3:4a39486ff238
- Child:
- 5:09afcbe0c18f
- Commit message:
- without multiserial
Changed in this revision
--- a/MultiSerial.lib Tue Mar 10 13:22:00 2015 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://developer.mbed.org/teams/B/code/MultiSerial/#f2f3c3d6a6be
--- a/main.cpp Tue Mar 10 13:22:00 2015 +0000
+++ b/main.cpp Wed Mar 11 01:03:00 2015 +0000
@@ -39,13 +39,15 @@
#include "mbed.h"
#include <math.h>
#include <sstream>
+
#include "Servo.h"
-#include "MultiSerial.h"
#include "PID.h"
#include "AQM0802A.h"
#include "main.h"
+//#include "MultiSerial.h"
+/*
void Receive(){
IrData[0] = rx_data[0];
IrData[1] = rx_data[1];
@@ -64,15 +66,21 @@
}
-
+*/
void move(int vr,int vl, double vs ,int Degree){
double pwm[4] = {0};
uint8_t i = 0;
- pwm[0] = vr - vs;
- pwm[1] = 0;
- pwm[2] = 0;
- pwm[3] = vl + vs;
-
+ if(vs<0){
+ pwm[0] = vr - vs;
+ pwm[1] = 0;
+ pwm[2] = 0;
+ pwm[3] = vl + vs;
+ }else{
+ pwm[0] = vr + vs;
+ pwm[1] = 0;
+ pwm[2] = 0;
+ pwm[3] = vl - vs;
+ }
for(i = 0; i < 4; i++){
if(pwm[i] > 100){
pwm[i] = 100;
@@ -284,7 +292,7 @@
}
/*それ以外*/
move(10,10,CompassPID,0);
- Receive();
+ //Receive();
if(!(IrNum == 0)) return;
move(40,40,CompassPID,0);
return ;
@@ -344,12 +352,11 @@
void PidUpdate()
{
- Compass = rx_data[7]+rx_data[8];
pid.setSetPoint((int)((REFERENCE + SetC) / 1.0));
InputPID = Compass;
pid.setProcessValue(InputPID);
- CompassPID = 0;//-(pid.compute());
+ CompassPID = (pid.compute());
}
@@ -401,7 +408,8 @@
Motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
Motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用）
-
+ Mbed.attach(&micon_rx,Serial::RxIrq); //送信空き割り込み（センサ用）
+
S555.calibrate(0.0005, 120.0);
//S555.position(0.0); //初期位置にセット
move(0,0,0,0);//停止
@@ -415,9 +423,7 @@
pid.setMode(AUTO_MODE); //pid sed def
pid.setSetPoint(REFERENCE); //pid sed def
pidupdate.attach(&PidUpdate, PID_CYCLE);
-
-
}
void StartLoop(){
@@ -450,7 +456,7 @@
}
if(State == Debug2){
while((State == Debug2)){
- Receive();
+ //Receive();
Lcd.printf("%d\n",IrNum);
wait_ms(100);
State = SwRead();
@@ -471,14 +477,13 @@
if(State == Kicker){
Led[0] = Led[1] = Led[2] = 0;
- move(20,20,0,0);
+ Kick = 1;
+ wait_ms(500);
+ Kick = 0;
while((State == Kicker)){
wait_ms(100);
State = SwRead();
-
}
- move(0,0,0,0);
-
continue;
}
}
@@ -519,20 +524,16 @@
SetUp();/*set up routine*/
-
- //Mbed.read_data(rx_data, ADDRESS);
- //Mbed.start_read();
- //Mbed.check_rx_wait();
-
+
StartLoop(); /*loop strat, switch push end*/
Led[0] = Led[1] = Led[2] = Led[3] = 0;
wait_ms(100);
- while(0){
-
+ while(1){
+ S555.calibrate(0.0005, 120.0);
- Receive();
+ //Receive();
//Lcd.printf("%d\n",IrNum);
/*白線を読んでいないか確認する*/
LineData = (~Line+0x00) & 0x0F;
@@ -544,7 +545,7 @@
move(0,0,0,0);
while(LineIr){
Led[1] = Led[2] = Led[3] = 1;
- Receive();
+ //Receive();
LineData = (~Line+0x00) & 0x0F;
LineIr = LineData & IrChange[IrNum];
wait_ms(10);
@@ -553,7 +554,7 @@
move(0,0,0,0);
while(LinePing){
Led[1] = Led[2] = Led[3] = 1;
- Receive();
+ //Receive();
LineData = (~Line+0x00) & 0x0F;
LinePing = PingChange(LineData);
@@ -573,7 +574,7 @@
Led[3] = 1;
- Receive();
+ //Receive();
Degree = IrDegree();
if((Degree == 0)||(Degree == 180)||(IrNum == 12)){
@@ -605,12 +606,14 @@
}
- int i;
- while(1){
+ while(0){
//デモプログラム
//Receive();
+ pc.printf("%d %d %d %d %d\n",IrData[0],IrData[1],IrData[2],PingData[0],PingData[1]);
+ //pc.printf("%d %d %d %d\n",PingData[1],PingData[2],PingData[3],Compass);
+
//pc.printf("%d\t %d\t %d\t %d\t %d\t %d\t\n",rx_data[3],rx_data[4],rx_data[5],rx_data[6],rx_data[7],rx_data[8]);
- pc.printf("%d\t %d\t %d\t %d\n",speed[0],speed[1],speed[2],speed[3]);
+ //pc.printf("%d\t %d\t %d\t %d\n",speed[0],speed[1],speed[2],speed[3]);
wait(0.1);
}
--- a/main.h Tue Mar 10 13:22:00 2015 +0000
+++ b/main.h Wed Mar 11 01:03:00 2015 +0000
@@ -1,10 +1,10 @@
/*PID処理*/
#define RATE 0.052//52
-#define PID_BIAS 0.3
+#define PID_BIAS 0.2
#define REFERENCE 180.0
-#define MINIMUM 0.1
-#define MAXIMUM 360.0
+#define MINIMUM 0.0
+#define MAXIMUM 359.0
#define P_GAIN 1.4//1.4 //0.78
#define I_GAIN 0.0 //0.0
#define D_GAIN 0.05 //0.009
@@ -29,16 +29,14 @@
#define ENTER 0
#define EXIT 1
-
+Serial Mbed(p9,p10);//tx,rx
BusIn Sw(p22,p23,p24,p25,p26);
BusIn Line(p5,p6,p7,p8);//No reading line → 0b1111 ,not pull up
DigitalOut Led[4] = {LED1,LED2,LED3,LED4};
DigitalOut Kick(p29);
Servo S555(p21);
-MultiSerial Mbed(p9,p10);//tx,rx
AQM0802A Lcd(p28,p27);//sda,scl
-
Serial Motor(p13,p14);//tx,rx
Serial pc(USBTX,USBRX);
@@ -47,13 +45,14 @@
PID pid(P_GAIN,I_GAIN,D_GAIN, RATE);
Ticker pidupdate;
-/* motor driver*/
+
extern string StringFIN;
extern void array(int,int,int,int);
+extern void micon_rx(void);
+
int speed[4] = {0};
-enum {FRONT = 1, RIGHT, BACK, LEFT, NA} Direction;
double SetDegree = 0;
@@ -63,7 +62,11 @@
unsigned int Compass = 0;
double SetC = 0, InputPID = 180.0, CompassPID = 0.0;
-uint8_t rx_data[9] = {0};
+
+/* motor driver*/
+
+enum {FRONT = 1, RIGHT, BACK, LEFT, NA} Direction;
+
/*mbed sensor data */
volatile int Angle[180] = {
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/usart.cpp Wed Mar 11 01:03:00 2015 +0000
@@ -0,0 +1,86 @@
+#include "mbed.h"
+
+
+#define KEYCODE 0xAA
+#define TX_CHECKCODE (tx_data[1]^tx_data[2]^tx_data[3]^tx_data[4]^tx_data[5]^tx_data[6]^tx_data[7]^tx_data[8]^tx_data[9])
+#define RX_CHECKCODE (rx_data[1]^rx_data[2]^rx_data[3]^rx_data[4]^rx_data[5]^rx_data[6]^rx_data[7]^rx_data[8]^rx_data[9])
+#define DATA_NUM 11
+#define CHECK (DATA_NUM - 1)
+
+
+extern Serial Mbed;
+extern Serial pc;
+
+extern uint8_t PingData[4];
+extern uint8_t IrData[3];
+extern uint8_t IrNum;
+extern unsigned int Compass;
+
+
+
+void micon_rx(){
+
+ static uint8_t rx;
+ static int rx_data[DATA_NUM];
+
+ rx_data[rx] = Mbed.getc();
+
+ if(rx_data[0] == KEYCODE){
+ rx++;
+ }
+
+ if(rx >= DATA_NUM){
+ if(rx_data[CHECK] == RX_CHECKCODE){
+ IrData[0] = rx_data[1];
+ IrData[1] = rx_data[2];
+ IrData[2] = rx_data[3];
+ PingData[0] = rx_data[4];
+ PingData[1] = rx_data[5];
+ PingData[2] = rx_data[6];
+ PingData[3] = rx_data[7];
+ Compass = rx_data[8] + rx_data[9];
+
+ if((IrData[0] == 255)||(IrData[1] == 255)||(IrData[2] == 255)){
+ IrNum = 12;
+
+ }else{
+ IrNum = IrData[0]/12;
+ }
+ //pc.printf("%d\t %d\t %d\t %d\t %d\t %d\t\n",rx_data[3],rx_data[4],rx_data[5],rx_data[6],rx_data[7],rx_data[8]);
+ //pc.printf("compass: %d\n",compass);
+ //pc.printf("ping0:%d\tping1:%d\tping2:%d\tping3:%d\n",ping[0],ping[1],ping[2],ping[3]);
+ //pc.printf("ir_min:%d\tir_num:%d\tir_main:%d\n",ir_min,ir_num,ir_main);
+
+ }
+ rx = 0;
+ }
+
+ //pc.printf("%d\n", rx_data[rx]);
+}
+/*
+void micon_tx(){
+
+ static uint8_t tx;
+ static uint8_t tx_data[DATA_NUM];
+
+ if(tx >= DATA_NUM){
+ tx_data[0] = KEYCODE;
+ tx_data[1] = KEYCODE;
+ tx_data[2] = KEYCODE;
+ tx_data[3] = KEYCODE;
+ tx_data[4] = KEYCODE;
+ tx_data[5] = KEYCODE;
+ tx_data[6] = KEYCODE;
+ tx_data[7] = KEYCODE;
+ tx_data[8] = KEYCODE;
+ tx_data[9] = KEYCODE;
+ tx_data[10] = KEYCODE;
+ tx_data[11] = TX_CHECKCODE;
+
+ tx = 0;
+ }
+
+ sensor.putc(tx_data[tx]);
+ tx++;
+}
+*/
\ No newline at end of file