ryo seki
aa
Dependencies: HMC6352 PID mbed
Diff: main.cpp
- Revision:
- 0:bde8ed56b164
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Jun 19 08:42:55 2013 +0000
@@ -0,0 +1,564 @@
+#include <math.h>
+#include <sstream>
+#include "mbed.h"
+#include "HMC6352.h"
+#include "PID.h"
+#include "main.h"
+
+
+
+void PidUpdate()
+{
+ static uint8_t Fflag = 0;
+
+ pid.setSetPoint(((int)((REFERENCE + standTu) + 360) % 360) / 1.0);
+ inputPID = (((int)(compass.sample() - (standard * 10.0) + 5400.0) % 3600) / 10.0);
+
+ //pc.printf("%f\n",timer1.read());
+ pid.setProcessValue(inputPID);
+ //timer1.reset();
+
+ compassPID = -(pid.compute());
+
+ if(!Fflag){
+ Fflag = 1;
+ compassPID = 0;
+ }
+ //pc.printf("%d\t%d\t%d\n",speed[0],speed[1],speed[2]);
+ //pc.printf("standard = \t\t%f\n",standard);
+ //pc.printf("%d\n",diff);
+ //pc.printf("compass.sample = \t%f\n",compass.sample() / 1.0);
+ //pc.printf("compassPID = \t\t%d\n",(int)compassPID);
+ //pc.printf("inputPID = \t\t%f\n\n",inputPID);
+
+ //pc.printf("%d\t%d\n",Distance,direction);
+ //pc.printf("%d\t%d\t%d\t%d\n",ultrasonicVal[0],ultrasonicVal[1],ultrasonicVal[2],ultrasonicVal[3]);
+}
+
+void IrDistanceUpdate()
+{
+ for(uint8_t i = 0;i < 6;i++)
+ {
+ AnalogIn adcIn(adc_num[i]); /* 今回更新する赤外線の個体を呼び出す */
+ irDistance[i] = adcIn.read_u16() >> 4;
+ //pc.printf("%d\n",irDistance[0]);
+ }
+}
+
+
+void move(int vxx, int vyy, int vss)
+{
+ double motVal[MOT_NUM] = {0};
+
+ motVal[0] = (double)(((0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT1);
+ motVal[1] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * vss)) * MOT2);
+ motVal[2] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT3);
+ motVal[3] = (double)(((0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * vss)) * MOT4);
+
+ for(uint8_t i = 0 ; i < MOT_NUM ; i++){
+ if(motVal[i] > MAX_POW)motVal[i] = MAX_POW;
+ else if(motVal[i] < MIN_POW)motVal[i] = MIN_POW;
+ speed[i] = (int)motVal[i];
+ }
+}
+
+int vector(double Amp,double degree,int xy)
+{
+ double result;
+
+ if(xy == X){
+ result = Amp * cos(degree * PI / 180.0);
+ }else if(xy == Y){
+ result = Amp * sin(degree * PI / 180.0) * (2.0 / sqrt(3.0));
+ }
+
+ return (int)result;
+}
+
+/*********** 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());
+}
+/*
+void Rx_interrupt()
+{
+ if(driver.readable()){
+ //pc.printf("%d\n",driver.getc());
+ }
+}*/
+
+
+/*********** Serial interrupt **********/
+
+
+void init()
+{
+ int scanfSuccess;
+ int printfSuccess;
+ int closeSuccess;
+ int close2Success;
+ uint8_t MissFlag = 0;
+ FILE *fp;
+
+ compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+
+ StartButton.mode(PullUp);
+ CalibEnterButton.mode(PullUp);
+ CalibExitButton.mode(PullUp);
+ EEPROMButton.mode(PullUp);
+
+ driver.baud(BAUD_RATE);
+ device2.baud(BAUD_RATE2);
+ wait_ms(MOTDRIVER_WAIT);
+ driver.printf("1F0002F0003F0004F000\r\n");
+ device2.printf("START");
+
+ driver.attach(&Tx_interrupt, Serial::TxIrq);
+ //driver.attach(&Rx_interrupt, Serial::RxIrq);
+ device2.attach(&dev_rx,Serial::RxIrq);
+ device2.attach(&dev_tx,Serial::TxIrq);
+
+ pid.setInputLimits(MINIMUM,MAXIMUM);
+ pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT);
+ pid.setBias(PID_BIAS);
+ pid.setMode(AUTO_MODE);
+ pid.setSetPoint(REFERENCE);
+
+ irDistanceUpdata.attach(&IrDistanceUpdate, 0.1);
+
+ Survtimer.start();
+
+ mbedleds = 1;
+
+ while(StartButton){
+ MissFlag = 0;
+ if(!CalibEnterButton){
+ mbedleds = 2;
+ compass.setCalibrationMode(ENTER);
+ while(CalibExitButton);
+ compass.setCalibrationMode(EXIT);
+ wait(BUT_WAIT);
+ mbedleds = 4;
+ }
+
+ if(!EEPROMButton){
+ Survtimer.reset();
+ fp = fopen("/local/out.txt", "r");
+ if(fp == NULL){
+ wait(BUT_WAIT);
+ MissFlag = 1;
+ }else{
+ scanfSuccess = fscanf(fp, "%lf",&standard);
+ if(scanfSuccess == EOF){
+ wait(BUT_WAIT);
+ MissFlag = 1;
+ }else{
+ closeSuccess = fclose(fp);
+ if(closeSuccess == EOF){
+ wait(BUT_WAIT);
+ MissFlag = 1;
+ }else{
+ wait(BUT_WAIT);
+ }
+ }
+ }
+ if((Survtimer.read() > (BUT_WAIT + 0.1)) || (MissFlag)){
+ for(uint8_t i = 0;i < 2;i++){
+ mbedleds = 15;
+ wait(0.1);
+ mbedleds = 0;
+ wait(0.1);
+ }
+ mbedleds = 15;
+ }else{
+ mbedleds = 8;
+ }
+ }
+
+ if(!CalibExitButton){
+ Survtimer.reset();
+
+ standard = compass.sample() / 10.0;
+
+ fp = fopen("/local/out.txt", "w");
+ if(fp == NULL){
+ wait(BUT_WAIT);
+ MissFlag = 1;
+ }else{
+ printfSuccess = fprintf(fp, "%f",standard);
+ if(printfSuccess == EOF){
+ wait(BUT_WAIT);
+ MissFlag = 1;
+ }else{
+ close2Success = fclose(fp);
+ if(close2Success == EOF){
+ wait(BUT_WAIT);
+ MissFlag = 1;
+ }else{
+ wait(BUT_WAIT);
+ }
+ }
+ }
+ if((Survtimer.read() > (BUT_WAIT + 0.2)) || (MissFlag)){
+ for(uint8_t i = 0;i < 4;i++){
+ mbedleds = 15;
+ wait(0.1);
+ mbedleds = 0;
+ wait(0.1);
+ }
+ mbedleds = 15;
+ }else{
+ mbedleds = 10;
+ }
+ }
+ }
+
+ mbedleds = 0;
+
+ Survtimer.stop();
+
+ for(uint8_t i = 0;i < 6;i++)
+ {
+ stand[i] = irDistance[i];
+ }
+ irDistanceUpdata.detach();
+
+ pidUpdata.attach(&PidUpdate, PID_CYCLE);
+ //irDistanceUpdata.attach(&IrDistanceUpdate, 0.1);
+ //timer1.start();
+}
+
+int main()
+{
+ int vx=0,vy=0,vs=0;
+ uint8_t whiteFlag = 0;
+
+ init();
+
+ while(1){
+ whiteFlag = 0;
+ hold_flag = 0;
+ vx = 0;
+ vy = 0;
+ vs = (int)compassPID;
+ //vs = 0;
+
+ //move(vx,vy,vs);
+
+ /*********************************************************************************************************
+ **********************************************************************************************************
+ ********************Change state *************************************************************************
+ **********************************************************************************************************
+ *********************************************************************************************************/
+ for(uint8_t i = 2;i < 6;i++)
+ {
+ AnalogIn adcIn(adc_num[i]); /* 今回更新する赤外線の個体を呼び出す */
+ irDistance[i] = ((adcIn.read_u16() >> 4) - stand[i]);
+ if(irDistance[i] >= 100)
+ {
+ whiteFlag = 1;
+ }
+ //pc.printf("%d\n",irDistance[0]);
+ }
+
+ if(lineState == NORMAL){
+ if((LEFT_SONIC < 350) && (whiteFlag)){
+ lineState = LEFT_OUT;
+ }else if((RIGHT_SONIC < 350) && (whiteFlag)){
+ lineState = RIGHT_OUT;
+ }else if((FRONT_SONIC < 400) && (whiteFlag)){
+ lineState = FRONT_OUT;
+ }else if((BACK_SONIC < 400) && (whiteFlag)){
+ lineState = BACK_OUT;
+ }
+ }else if(lineState == LEFT_OUT){
+ if((LEFT_SONIC > 450) && (!whiteFlag)){
+ lineState = NORMAL;
+ }
+ }else if(lineState == RIGHT_OUT){
+ if((RIGHT_SONIC > 450) && (!whiteFlag)){
+ lineState = NORMAL;
+ }
+ }else if(lineState == FRONT_OUT){
+ if((FRONT_SONIC > 500) && (!whiteFlag)){
+ lineState = NORMAL;
+ }
+ }else if(lineState == BACK_OUT){
+ if((BACK_SONIC > 500) && (!whiteFlag)){
+ lineState = NORMAL;
+ }
+ }
+
+
+ if(state == HOLD){
+ if(FRONT_SONIC < 100)hold_flag = 1;
+
+ if(Distance > 140){ //審判のボール持ち上げ判定
+ state = HOME_WAIT;
+ }else if(!((direction == 0) || (direction == 15) || (direction == 1) || (direction == 14) || (direction == 2))){//ボールを見失った
+ state = DIFFENCE;
+ }else if(!(Distance <= 40)){//ボールを見失った
+ state = DIFFENCE;
+ }
+ }else{
+ standTu = 0;
+ if(state == DIFFENCE){
+ if((direction == 0) && (Distance <= 20) && (IR_found) && (!xbee)){
+ state = HOLD;
+ }else if((Distance < 180) && (IR_found) && (!xbee)){
+ state = DIFFENCE;
+ }else{
+ if((direction == 4) || (direction == 6) || (direction == 8) || (direction == 10)|| (direction == 12)){
+ if((IR_found) && (!xbee)){
+ state = DIFFENCE;
+ }
+ }else if((diff > 15) && (!xbee) && (IR_found)){
+ state = DIFFENCE;
+ }else{
+ state = HOME_WAIT;
+ }
+ }
+
+ }else{
+ if((direction == 0) && (Distance <= 30) && (IR_found) && (!xbee)){
+ state = HOLD;
+ }else if((Distance <= 150) && (IR_found) && (!xbee)){
+ state = DIFFENCE;
+ }else{
+ if((direction == 4) || (direction == 6) || (direction == 8) || (direction == 10)|| (direction == 12)){
+ if((IR_found) && (!xbee)){
+ state = DIFFENCE;
+ }
+ }else if((diff > 15) && (!xbee) && (IR_found)){
+ state = DIFFENCE;
+ }else{
+ state = HOME_WAIT;
+ }
+ }
+ }
+ }
+ /**/
+ /*********************************************************************************************************
+ **********************************************************************************************************
+ ********************Change state *************************************************************************
+ **********************************************************************************************************
+ *********************************************************************************************************/
+
+ //state = HOME_WAIT;
+ if(state == HOME_WAIT){
+ mbedleds = 1;
+ /*
+ if(((RIGHT_SONIC + LEFT_SONIC) < 1100.0) && ((RIGHT_SONIC + LEFT_SONIC) > 850.0)){
+ if((LEFT_SONIC > 425.0) && (RIGHT_SONIC > 425.0)){
+ vx = 0;
+ }else if(RIGHT_SONIC < 425.0){
+ vx = (int)((RIGHT_SONIC - 425.0) * 0.02 - 10.0);
+ if(vx < -15)vx = -15;
+ }else if(LEFT_SONIC < 425.0){
+ vx = (int)((425.0 - LEFT_SONIC ) * 0.02 + 10.0);
+ if(vx > 15)vx = 15;
+ }
+
+ if((RIGHT_SONIC < 330.0) || (LEFT_SONIC < 330.0)){
+ if((BACK_SONIC > 15.0) && (BACK_SONIC < 45.0)){
+ vy = 0;
+ }else if((BACK_SONIC <= 15.0) || (BACK_SONIC == PING_ERROR)){
+ vy = 5;
+ }else{
+ vy = (int)(0.015 * (30.0 - BACK_SONIC) - 4);
+ if(vy < -10)vy = -10;
+ }
+ }else{
+ if((BACK_SONIC > 95.0) && (BACK_SONIC < 125.0)){
+ vy = 0;
+ }else if((BACK_SONIC <= 95.0) || (BACK_SONIC == PING_ERROR)){
+ vy = 5;
+ }else{
+ vy = (int)(0.015 * (110.0 - BACK_SONIC) - 4);
+ if(vy < -10)vy = -10;
+ }
+ }
+ }else if((RIGHT_SONIC + LEFT_SONIC) <= 850.0){
+ if(BACK_SONIC < 200.0){
+ if(RIGHT_SONIC > LEFT_SONIC){
+ vx = 15;
+ vy = 5;
+ }else{
+ vx = -15;
+ vy = 5;
+ }
+ }else{
+ vx = 0;
+ vy = -10;
+ }
+ }else{
+ if(BACK_SONIC > 500.0){
+ if(RIGHT_SONIC > LEFT_SONIC){
+ vx = 10;
+ vy = -10;
+ }else{
+ vx = -10;
+ vy = -10;
+ }
+ }
+ }
+ */
+ }else if(state == DIFFENCE){
+ mbedleds = 4;
+ if((direction == 6) || (direction == 4)){
+ vx = 20;
+
+ if(LEFT_SONIC < 500){
+ if((BACK_SONIC > 370) && (BACK_SONIC < 400)){
+ vy = 0;
+ }else if((BACK_SONIC <= 370) || (BACK_SONIC == PING_ERROR)){
+ vy = 5;
+ }else{
+ vy = (int)(0.015 * (400.0 - BACK_SONIC) - 4);
+ if(vy < -15)vy = -15;
+ }
+ }else if(RIGHT_SONIC < 500){
+ vx = 15;
+ vy = -10;
+ }else{
+ if((BACK_SONIC > 70) && (BACK_SONIC < 100)){
+ vy = 0;
+ }else if((BACK_SONIC <= 70) || (BACK_SONIC == PING_ERROR)){
+ vy = 5;
+ }else{
+ vy = (int)(0.015 * (100.0 - BACK_SONIC) - 4);
+ if(vy < -15)vy = -15;
+ }
+ }
+ }else if((direction == 10) || (direction == 12)){
+ vx = -20;
+
+ if(RIGHT_SONIC < 500){
+ if((BACK_SONIC > 370) && (BACK_SONIC < 400)){
+ vy = 0;
+ }else if((BACK_SONIC <= 370) || (BACK_SONIC == PING_ERROR)){
+ vy = 5;
+ }else{
+ vy = (int)(0.015 * (400.0 - BACK_SONIC) - 4);
+ if(vy < -15)vy = -15;
+ }
+ }else if(LEFT_SONIC < 500){
+ vx = -15;
+ vy = -10;
+ }else{
+ if((BACK_SONIC > 70) && (BACK_SONIC < 100)){
+ vy = 0;
+ }else if((BACK_SONIC <= 70) || (BACK_SONIC == PING_ERROR)){
+ vy = 5;
+ }else{
+ vy = (int)(0.015 * (100.0 - BACK_SONIC) - 4);
+ if(vy < -15)vy = -15;
+ }
+ }
+
+ }else if(direction == 8){
+
+ if(LEFT_SONIC > RIGHT_SONIC){
+ vx = -20;
+ }else{
+ vx = 20;
+ }
+ if((RIGHT_SONIC < 500) || (LEFT_SONIC < 500)){
+ if(BACK_SONIC < 500){
+ vy = -4;
+ }else{
+ vy = (int)(0.015 * (500.0 - BACK_SONIC) - 4);
+ if(vy < -15)vy = -15;
+ }
+ }else{
+ if(BACK_SONIC < 200){
+ vy = -4;
+ }else{
+ vy = (int)(0.015 * (200.0 - BACK_SONIC) - 4);
+ if(vy < -15)vy = -15;
+ }
+ }
+
+ }else if(direction == 2){
+
+ vx = 25;
+ vy = 0; //0
+
+ }else if(direction == 14){
+
+ vx = -25;
+ vy = 0; //-4
+
+ }else if(direction == 1){
+
+
+ vx = 20;
+ vy = 0; //0
+
+
+ }else if(direction == 15){
+
+ vx = -20;
+ vy = 0; //-3
+
+ }else if(direction == 0){
+
+ vx = 0;
+ vy = 20;
+
+ }else{//error
+
+ vx = 0;
+ vy = 0;
+
+ }
+ }else if(state == HOLD){
+ mbedleds = 15;
+
+ vy = 20;
+
+ if(((RIGHT_SONIC + LEFT_SONIC) < 1800.0) && ((RIGHT_SONIC + LEFT_SONIC) > 1400.0)){
+ standTu = (RIGHT_SONIC - LEFT_SONIC) / 25.0;
+ }
+ }
+
+ if(lineState == NORMAL){
+ //mbedleds = 1;
+
+ }else if(lineState == LEFT_OUT){
+ //mbedleds = 2;
+
+ vx = 30;
+ }else if(lineState == RIGHT_OUT){
+ //mbedleds = 4;
+
+ vx = -30;
+ }else if(lineState == FRONT_OUT){
+ //mbedleds = 8;
+
+ vy = -40;
+ }else if(lineState == BACK_OUT){
+ //mbedleds = 12;
+
+ vy = 40;
+ }
+ //vx = vector(10,45,X);
+ //vy = vector(10,45,Y);
+ //vx = 40;
+ //vy = 0;
+ //pc.printf("%d\t%d\n",vx,vy);
+
+ //vy = -3;
+ //vs = 0;
+ //vx = 0;
+ //vx = 10;
+ //vx = 25;
+ //vy = 0;
+
+ move(vx,vy,vs);
+ }
+}
\ No newline at end of file