ryo seki
test
Revision 0:74bf4953c0d1, committed 2013-03-09
- Comitter:
- akudohune
- Date:
- Sat Mar 09 10:11:06 2013 +0000
- Commit message:
- ver1_2_2;
Changed in this revision
diff -r 000000000000 -r 74bf4953c0d1 HMC6352.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/HMC6352.lib Sat Mar 09 10:11:06 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/HMC6352/#83c0cb554099
diff -r 000000000000 -r 74bf4953c0d1 IR.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/IR.cpp Sat Mar 09 10:11:06 2013 +0000
@@ -0,0 +1,205 @@
+
+#include "mbed.h"
+#include "IR.h"
+
+
+extern Timer timer_ir;
+extern Serial pc; // tx, rx
+
+int direction = 0;
+int Distance = 0;
+
+int IR_found;
+
+
+void IR_Position(){
+
+ int ir_value[ALL_IR+100] = {0};
+
+ int active_ir = 0; /* 今回更新する赤外線の番号 */
+ int memory_ir = 0; /*赤外線時間カウンタ*/
+ int flag_ir = 0;
+ int value = 0;
+
+ for(int i=0; i<ALL_IR; i++) {
+ flag_ir = 1;
+
+ DigitalIn sensor_ir(ir_num[active_ir]); /* 今回更新する赤外線の個体を呼び出す */
+
+ timer_ir.start(); /* タイマー起動 */
+
+ if(sensor_ir) { /* もし立ち上がっていたら */
+ while(sensor_ir) { /* 立ち下がるまで待つ */
+ if(timer_ir.read_us() >= IR_TIME_NOTFOUND) {
+ flag_ir = 0;
+ break; /* 立ち上がっている時間が指定時間越えたらブレイク */
+ }
+ }
+ }
+
+ timer_ir.stop(); /* タイマー停止 */
+ timer_ir.reset(); /* タイマーリセット */
+
+ if(flag_ir) {
+ timer_ir.start(); /* タイマー起動 */
+
+ while(!(sensor_ir)) { /* 立ち上がるまで待つ */
+ if(timer_ir.read_us() >= IR_TIME_NOTFOUND) {
+ flag_ir = 0;
+ break; /* 立ち上がっている時間が指定時間越えたらブレイク */
+ }
+ }
+ }
+
+ /*ボールが指定時間内に見つかっていたら*/
+ if(flag_ir) {
+ memory_ir = timer_ir.read_us();
+
+ while(1) {
+ if((timer_ir.read_us()-memory_ir)>=IR_TIME_NOTFOUND)break;
+
+ if(!(sensor_ir)) {
+ //value = moving_ave( (timer_ir.read_us()-memory_ir)/10 , active_ir );
+ value = (timer_ir.read_us()-memory_ir)/10;
+
+ break;
+ }
+ }
+ } else {
+ /*ボールが見つかっていない場合*/
+ value = 0;
+ }
+ timer_ir.stop(); /* タイマー停止 */
+ timer_ir.reset(); /* タイマーリセット */
+
+ memory_ir = 0;
+
+ ir_value[active_ir] = value; //direction array
+
+ active_ir++;
+
+ if( active_ir >= ALL_IR) {
+ active_ir = 0;
+
+ /***********direction***********/
+
+ int min = 100,youso_min = 100;
+
+ for(int i = 0; i<DIREC_IR; i++) {
+ if((ir_value[i]<min)&&(ir_value[i])) {
+ min = ir_value[i];
+ youso_min = i;
+ }
+ }
+
+ double hiritu = 0;
+
+ int direc = 0;
+
+ if(youso_min == 0) {
+ hiritu = (double)ir_value[7]/(double)ir_value[1];
+ } else if(youso_min ==7) {
+ hiritu = (double)ir_value[6]/(double)ir_value[0];
+ } else {
+ hiritu = (double)ir_value[youso_min-1]/(double)ir_value[youso_min+1];
+ }
+
+ if((hiritu <= 0.85)&&(youso_min != 0)) {
+ direc = youso_min*2-1;
+ }else if((hiritu <= 0.85)&&(youso_min == 0)){
+ direc = 15;
+ }else if(hiritu >= 1.15) {
+ direc = youso_min*2+1;
+ } else {
+ direc = youso_min*2;
+ }
+
+ /*if(youso_min == 0){
+ direc = 0;
+ }*/
+
+ /******* direction end *******/
+
+ /******* distance *******/
+
+ int dista;
+
+ if((ir_value[youso_min]<=28 + TERM)) {
+ dista = 30;
+ } else if((ir_value[youso_min]>28 + TERM)&&(ir_value[youso_min]<=35 + TERM)) {
+ dista = 90;
+ } else if((ir_value[youso_min]>35 + TERM)&&(ir_value[youso_min]<=40 + TERM)) {
+ dista = 120;
+ } else if( ir_value[youso_min]>40 + TERM) {
+ dista = 180;
+ } else {
+ dista = 0;
+ }
+
+ int count_ir = 0,total_ir = 0;
+
+ for(int i=0; i<DIREC_IR; i++){
+ if(ir_value[i]){
+ total_ir += ir_value[i];
+ count_ir++;
+ }
+ }
+
+ double hihhihi = 0;
+
+ hihhihi = (double)ir_value[youso_min]/(double)ir_value[8];
+
+
+ if((direc == 0)&&(hihhihi >= 0.80)){
+ dista = 10;
+ }else if((direc == 1)&&(hihhihi >= 0.80)){
+ dista = 10;
+ }else if((direc == 2)&&(hihhihi >= 0.65)){
+ dista = 10;
+ }else if((direc == 3)&&(hihhihi >= 0.65)){
+ dista = 10;
+ }else if((direc == 4)&&(hihhihi >= 0.80)){
+ dista = 10;
+ }else if((direc == 5)&&(hihhihi >= 0.65)){
+ dista = 10;
+ }else if((direc == 6)&&(hihhihi >= 0.65)){
+ dista = 10;
+ }else if((direc == 7)&&(hihhihi >= 0.80)){
+ dista = 10;
+ }else if((direc == 8)&&(hihhihi >= 0.80)){
+ dista = 10;
+ }else if((direc == 9)&&(hihhihi >= 0.80)){
+ dista = 10;
+ }else if((direc == 10)&&(hihhihi >= 0.65)){
+ dista = 10;
+ }else if((direc == 11)&&(hihhihi >= 0.65)){
+ dista = 10;
+ }else if((direc == 12)&&(hihhihi >= 0.80)){
+ dista = 10;
+ }else if((direc == 13)&&(hihhihi >= 0.65)){
+ dista = 10;
+ }else if((direc == 14)&&(hihhihi >= 0.65)){
+ dista = 10;
+ }else if((direc == 15)&&(hihhihi >= 0.80)){
+ dista = 10;
+ }
+
+ int count = 0;
+
+ for(int i=0;i<DIREC_IR;i++){
+ if(ir_value[i])count++;
+ }
+
+ if(count) IR_found = 1;
+ else IR_found = 0;
+
+
+ /******** distance end *******/
+
+ direction = direc;
+ Distance = dista;
+
+ //printf("derection:%d distance:%d\n",direction,Distance);
+ }
+ }
+}
\ No newline at end of file
diff -r 000000000000 -r 74bf4953c0d1 IR.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/IR.h Sat Mar 09 10:11:06 2013 +0000
@@ -0,0 +1,63 @@
+
+
+#define IR_TIME_NOTFOUND 833 /* 見つけられなかったと判断するまでの時間(単位：us) */
+#define IR_COUNTMAX 487 /*最大パルス幅 パルスの存在しうる最大時間は487us*/
+#define ALL_IR 9
+#define DIREC_IR 8
+#define DIRECTION 16
+#define TERM 0
+#define DELTA -2
+#define SWAP(type,a,b) { type temp = a; a = b; b = temp; }
+
+
+/* 赤外線センサに使うpinを配列に格納 */
+PinName ir_num[ALL_IR] = {
+ p13,
+ p14,
+ p15,
+ p16,
+ p17,
+ p18,
+ p19,
+ p20,
+ p30
+};
+
+int Convert_Direction[DIRECTION] = {
+ 90,
+ 67,
+ 45,
+ 22,
+ 0,
+ 337,
+ 315,
+ 292,
+ 270,
+ 247,
+ 225,
+ 202,
+ 180,
+ 157,
+ 135,
+ 112
+};
+
+//ball direction
+double ball_sankaku[16][2] = {
+ {0 , 1 },
+ {0.390 , 0.920},
+ {0.707 , 0.707},
+ {0.927 , 0.374},
+ {1 , 0 },
+ {0.920 ,-0.390},
+ {0.707 ,-0.707},
+ {0.374 ,-0.927},
+ {0 ,-1 },
+ {-0.390,-0.920},
+ {-0.707,-0.707},
+ {-0.927,-0.374},
+ {-1 , 0 },
+ {-0.920, 0.390},
+ {-0.707, 0.707},
+ {-0.374, 0.927}
+};
diff -r 000000000000 -r 74bf4953c0d1 PID.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.lib Sat Mar 09 10:11:06 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/PID/#6e12a3e5af19
diff -r 000000000000 -r 74bf4953c0d1 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sat Mar 09 10:11:06 2013 +0000
@@ -0,0 +1,214 @@
+#include <math.h>
+#include <sstream>
+#include "mbed.h"
+#include "HMC6352.h"
+#include "PID.h"
+#include "main.h"
+
+
+/*********** Serial interrupt ***********/
+
+void Tx_interrupt()
+{
+ array(speed[0],speed[1],speed[2],speed[3]);
+ driver.printf("%s",StringFIN.c_str());
+ //pc.printf("%s",StringFIN.c_str());
+ //pc.printf("compass.sample = %f\n",compass.sample() / 1.0);
+}
+/*
+void Rx_interrupt()
+{
+ if(driver.readable()){
+ //pc.printf("%d\n",driver.getc());
+ }
+}*/
+
+
+/*********** Serial interrupt end **********/
+
+void PidUpdata()
+{
+
+ if(standard < 180.0){
+ if((compass.sample() / 10.0) < standard){
+ inputPID = 180.0 -(standard - (compass.sample() / 10.0));
+ }else if((compass.sample() / 10.0) < 180.0 + standard){
+ inputPID = 180.0 +((compass.sample() / 10.0) - standard);
+ }else{
+ inputPID = 180.0 - ((360.0 - (compass.sample() / 10.0)) + standard);
+ }
+ }else if(standard > 180.0){
+ if((compass.sample() / 10.0) > standard){
+ inputPID = 180.0 +((compass.sample() / 10.0) - standard);
+ }else if((compass.sample() / 10.0) > standard - 180.0){
+ inputPID = 180.0 -(standard - (compass.sample() / 10.0));
+ }else{
+ inputPID = 180.0 + ((compass.sample() / 10.0) + (360.0 - standard));
+ }
+ }else{
+ inputPID = compass.sample() / 10.0;
+ }
+
+ if(inputPID < 0){
+ inputPID *= -1;
+ }
+
+ //pc.printf("%f\n",timer1.read());
+ pid.setProcessValue(inputPID);
+ //timer1.reset();
+
+ compassPID = -(pid.compute());
+
+ //pc.printf("%f\n",compassPID);
+
+}
+
+
+
+void move(int vx, int vy, int vs)
+{
+ double motVal[MOT_NUM] = {0};
+
+ motVal[0] = (double)(((0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + (Long * -vs)) * MOT1);
+ motVal[1] = (double)(((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + (Long * vs)) * MOT2);
+ motVal[2] = (double)(((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + (Long * -vs)) * MOT3);
+ motVal[3] = (double)(((0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + (Long * vs)) * MOT4);
+
+ for(uint8_t i = 0;i < MOT_NUM;i++){
+ if(motVal[i] > 100)motVal[i] = 100;
+ else if(motVal[i] < -100)motVal[i] = -100;
+ speed[i] = motVal[i];
+ }
+ /*
+ pc.printf("speed1 = %d\n",speed[0]);
+ pc.printf("speed2 = %d\n",speed[1]);
+ pc.printf("speed3 = %d\n",speed[2]);
+ pc.printf("speed4 = %d\n\n",speed[3]);
+ */
+ ////pc.printf("%s",StringFIN.c_str());
+}
+
+void init()
+{
+
+ compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+ StartButton.mode(PullUp);
+ CalibEnterButton.mode(PullUp);
+ CalibExitButton.mode(PullUp);
+ driver.baud(BAUD_RATE);
+ wait_ms(MOTDRIVER_WAIT);
+ driver.printf("1F0002F0003F0004F000\r\n");
+
+ led1 = ON;
+
+ while(StartButton){
+ if(!CalibEnterButton){
+ led1 = OFF;
+ led2 = ON;
+ compass.setCalibrationMode(ENTER);
+ while(CalibExitButton);
+ compass.setCalibrationMode(EXIT);
+ led2 = OFF;
+ led3 = ON;
+ }
+ }
+
+ standard = compass.sample() / 10.0;
+ led1 = OFF;
+ led3 = OFF;
+
+ pid.setInputLimits(0.0, 360.0);
+ pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT);
+ pid.setBias(0.0);
+ pid.setMode(AUTO_MODE);
+ pid.setSetPoint(180.0);
+
+ pidUpdata.attach(&PidUpdata, 0.06);
+ wait(1);
+ IR.attach(&IR_Position,0.04);
+ ultrasonic.attach(&Ultrasonic, 0.05);
+ driver.attach(&Tx_interrupt, Serial::TxIrq);
+ //driver.attach(&Rx_interrupt, Serial::RxIrq);
+
+ timer1.start();
+ timer2.start();
+}
+
+int main()
+{
+ int vx=0,vy=0,vs=0;
+ int state = HOME_WAIT;
+
+ init();
+
+ while(1) {
+
+ vs = compassPID;
+ //vx = 15;
+ //vy = 10;
+ /*
+ if(IR_found){
+ if((direction == 4) || (direction == 12)||(direction == 3) || (direction == 5) || (direction ==11) || (direction == 13)){
+ vx = (int)(20*ball_sankaku[direction][0]);
+ vy = (int)(20*ball_sankaku[direction][1]);
+ }else{
+ vx = (int)(10*ball_sankaku[direction][0]);
+ vy = (int)(10*ball_sankaku[direction][1]);
+ }
+
+ if(Distance <= 10){
+ vx *= -1;
+ vy *= -1;
+ }
+ }else{
+ vx = 0;
+ vy = 0;
+ }
+ */
+ /*
+ if((ultrasonicVal[2] < 700) && (inputPID < 190) && (inputPID > 170))vx = -15;
+ else vx = 0;
+ */
+ /*
+ if(IR_found)state = DIFFENCE;
+ else state = HOME_WAIT;
+
+
+ if(state == HOME_WAIT){
+ if((ultrasonicVal[0] + ultrasonicVal[2]) < 6000){
+ if(ultrasonicVal[0] > 3200){
+ vx = 15;
+ vy = 0;
+ }else if(ultrasonicVal[2] > 3200){
+ vx = -15;
+ vy = 0;
+ }else{
+ if(ultrasonicVal[1] > 700){
+ vx = 0;
+ vy = -15;
+ }else{
+ vx = 0;
+ vy = 0;
+ }
+ }
+ }else{
+ vx = 0;
+ vy = 0;
+ }
+ }else if(state == DIFFENCE){
+ if((direction == 1) || (direction == 2) || (direction == 3) || (direction == 4) || (direction == 5) || (direction == 6) || (direction == 7)){
+ vx = 15;
+
+ }else if((direction == 9) || (direction == 10) || (direction == 11) || (direction == 12) || (direction == 13) || (direction == 14) || (direction == 15)){
+ vx = -15;
+
+ }else{
+ vx = 0;
+
+ }
+ }
+ */
+
+ move(vx,vy,vs);
+ }
+}
diff -r 000000000000 -r 74bf4953c0d1 main.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.h Sat Mar 09 10:11:06 2013 +0000
@@ -0,0 +1,62 @@
+
+
+#define RATE 0.052
+#define Long 1.0
+#define ENTER 0
+#define EXIT 1
+#define MOT_NUM 4
+#define MOTDRIVER_WAIT 300 //ms
+#define BAUD_RATE 115200
+#define ON 1
+#define OFF 0
+
+#define MOT1 1
+#define MOT2 1
+#define MOT3 1
+#define MOT4 1
+
+#define OUT_LIMIT 30.0
+
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+HMC6352 compass(p9, p10);
+Serial driver(p28, p27); // tx, rx
+Serial pc(USBTX, USBRX); // tx, rx
+DigitalIn StartButton(p21);
+DigitalIn CalibEnterButton(p22);
+DigitalIn CalibExitButton(p23);
+PID pid(0.42, 1.0, 0.013, RATE); //30.0 0.35 1.0 0.012
+Ticker pidUpdata;
+Ticker IR;
+Ticker ultrasonic;
+Timer timer1;
+Timer timer2;
+Timer timer_ir; /* 赤外線用タイマー */
+
+
+int speed[MOT_NUM] = {0};
+
+static float lastData = 0.0;
+static float inputPID = 180.0;
+static float standard;
+static float compassPID = 0.0;
+
+extern string StringFIN;
+
+extern int direction;
+extern int Distance;
+extern int IR_found;
+extern double ball_sankaku[16][2];
+extern uint16_t ultrasonicVal[3];
+
+extern void Ultrasonic(void);
+extern void IR_Position(void);
+extern void PidUpdata(void);
+extern void array(int,int,int,int);
+
+enum{
+ HOME_WAIT,
+ DIFFENCE,
+};
+
diff -r 000000000000 -r 74bf4953c0d1 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sat Mar 09 10:11:06 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/5e5da4a5990b \ No newline at end of file
diff -r 000000000000 -r 74bf4953c0d1 ultrasonic.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ultrasonic.cpp Sat Mar 09 10:11:06 2013 +0000
@@ -0,0 +1,45 @@
+
+#include "mbed.h"
+#include "ultrasonic.h"
+
+
+extern Timer timer2;
+extern Serial pc; // tx, rx
+
+uint16_t ultrasonicVal[ALL_ULTRASONIC] = {0};
+
+
+void Ultrasonic()
+{
+ for(uint8_t i = 0 ; i < ALL_ULTRASONIC ; i++){
+ uint8_t flag = 0;
+
+ DigitalOut PingPinOut(ultrasonic_pin[i]);
+ PingPinOut = 1;
+ wait_us(10);
+ PingPinOut = 0;
+ DigitalIn PingPin(ultrasonic_pin[i]);
+ timer2.reset();
+ while(PingPin == 0){
+ if(timer2.read_us() > 1000){ //1ms以上応答なし
+ ultrasonicVal[i] = PING_ERR;
+ flag = 1;
+ break;
+ }
+ }
+ timer2.reset();
+ while(PingPin == 1){
+ if((timer2.read_us() > 18500) || (flag == 1)){ //18.5ms以上のパルス
+ ultrasonicVal[i] = PING_ERR;
+ flag = 1;
+ break;
+ }
+ }
+ if(flag == 0){
+ ultrasonicVal[i] = timer2.read_us();
+ }
+ //pc.printf("compass.sample = %f\n",compass.sample() / 1.0);
+ }
+ pc.printf("%d\n",ultrasonicVal[0] + ultrasonicVal[2]);
+
+}
diff -r 000000000000 -r 74bf4953c0d1 ultrasonic.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ultrasonic.h Sat Mar 09 10:11:06 2013 +0000
@@ -0,0 +1,12 @@
+
+
+#define ALL_ULTRASONIC 3
+#define PING_ERR 0xFFFF
+
+
+PinName ultrasonic_pin[ALL_ULTRASONIC] = {
+ p25,
+ p26,
+ p29,
+ //p30,
+};
diff -r 000000000000 -r 74bf4953c0d1 wordString.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/wordString.cpp Sat Mar 09 10:11:06 2013 +0000
@@ -0,0 +1,70 @@
+
+#include <sstream>
+#include "mbed.h"
+
+string StringFIN;
+
+using namespace std;
+
+
+extern Serial pc; // tx, rx
+
+string IntToString(int number)
+{
+ stringstream ss;
+ ss << number;
+ return ss.str();
+}
+
+void array(int power1,int power2,int power3,int power4)
+{
+ int input[4] = {power1,power2,power3,power4};
+ int value = 0;
+ string StringA[4] = {"0","0","0","0"};
+
+
+ string StringX = "0";
+ string StringY = "0";
+ string StringZ = "0";
+ string String0 = "0";
+
+ StringFIN = "0";
+
+ for(uint8_t i = 0 ; i < 4; i++){
+
+ value = input[i];
+
+ StringX = IntToString(i+1);
+
+ if( (value < 0) && (value >= -100) ){
+ StringY = "R";
+ value = abs(value);
+ StringZ = IntToString(value);
+ }else if( (value >= 0) && (value <= 100) ){
+ StringY = "F";
+ StringZ = IntToString(value);
+ }else{
+ StringA[i] = StringX + "F" + "000";
+
+ }
+
+ if(value < 10){
+ String0 = "00";
+ StringZ = String0 + StringZ;
+ }else if(value < 100)
+ {
+ String0 = "0";
+ StringZ = String0 + StringZ;
+ }else{
+
+ }
+
+ StringA[i] = (StringX + StringY + StringZ);
+
+ if(i == 0)StringFIN = StringA[i];
+ else StringFIN += StringA[i];
+
+ }
+
+ StringFIN += "\r\n";
+}
\ No newline at end of file