ryo seki
a
Dependencies: HMC6352 PID mbed
Diff: main.cpp
- Revision:
- 0:8215f0743d86
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Apr 19 09:13:32 2013 +0000
@@ -0,0 +1,490 @@
+#include <math.h>
+#include <sstream>
+#include "mbed.h"
+#include "HMC6352.h"
+#include "PID.h"
+#include "main.h"
+
+
+
+void PidUpdata()
+{
+ 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());
+ //pc.printf("compass.sample = %f\n",compass.sample() / 1.0);
+ //pc.printf("%f\n",compassPID);
+}
+
+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] = motVal[i];
+ }
+ //pc.printf("%s",StringFIN.c_str());
+}
+
+/*********** 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 init()
+{
+ uint8_t initFlag = 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);
+ wait_ms(MOTDRIVER_WAIT);
+ driver.printf("1F0002F0003F0004F000\r\n");
+ device2.printf("START");
+
+ device2.attach(&dev_rx,Serial::RxIrq);
+ device2.attach(&dev_tx,Serial::TxIrq);
+
+ led1 = ON;
+
+ while(StartButton){
+ if(!CalibEnterButton){
+ led1 = OFF;
+ led2 = ON;
+ compass.setCalibrationMode(ENTER);
+ while(CalibExitButton);
+ compass.setCalibrationMode(EXIT);
+ led2 = OFF;
+ led3 = ON;
+ }
+ if(!EEPROMButton){
+ led3 = OFF;
+ led1 = OFF;
+ led4 = ON;
+ initFlag = 1;
+ fp = fopen("/local/out.txt", "r");
+ fscanf(fp, "%lf",&standard);
+ fclose(fp);
+ }
+ }
+
+ if(!initFlag){
+ standard = compass.sample() / 10.0;
+ fp = fopen("/local/out.txt", "w");
+ fprintf(fp, "%f",standard);
+ fclose(fp);
+ }
+
+ led1 = OFF;
+ led3 = OFF;
+ led4 = 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_ms(500); //danger
+ driver.attach(&Tx_interrupt, Serial::TxIrq);
+ //driver.attach(&Rx_interrupt, Serial::RxIrq);
+
+ //timer1.start();
+}
+
+int main()
+{
+ int vx=0,vy=0,vs=0;
+ uint8_t flag = 0;
+ uint8_t comp_flag6 = 0;
+ uint8_t comp_flag10 = 0;
+
+ init();
+
+ while(1){
+ //pc.printf("compass.sample = %f\n",compass.sample() / 1.0);
+ //wait(0.1);
+ //pc.printf("%d\t,%d\n",direction,Distance);
+
+ //pc.printf("%d\n",distance);
+
+ vx = 0;
+ vy = 0;
+ vs = compassPID;
+
+ /*************** Change state **************************/
+
+ if((Distance <= 30) && (IR_found) && (!flag)){
+ state = DIFFENCE;
+ }else{
+ if((direction == 4) || (direction == 6)|| (direction == 8) || (direction == 10)|| (direction == 12)){
+ state = DIFFENCE;
+ }else if((Distance <= 120) && (IR_found) && (!flag)){
+ state = WARNING;
+ }else{
+ state = HOME_WAIT;
+ }
+ }
+
+ if((IR_found) && (!flag)){
+ state = WARNING;
+ }else{
+ state = HOME_WAIT;
+ }
+
+ /*************** Change state **************************/
+
+
+ if(state == HOME_WAIT){
+ comp_flag6 = 0;
+ comp_flag10 = 0;
+ if(((RIGHT_SONIC + LEFT_SONIC) < 1050.0) && ((RIGHT_SONIC + LEFT_SONIC) > 600.0)){
+ if((BACK_SONIC > 115.0) && (BACK_SONIC < 125.0)){
+ vy = 0;
+ flag = 0;
+ }else if(BACK_SONIC <= 115.0){
+ vy = 3;
+ }else if(BACK_SONIC == PING_ERROR){
+ vy = 5;
+ }else{
+ vy = (int)(0.05 * (120.0 - BACK_SONIC) - 4);
+ if(vy < -30)vy = -30;
+ }
+
+ if((LEFT_SONIC > 450.0) && (RIGHT_SONIC > 450.0)){
+ vx = 0;
+ }else if(RIGHT_SONIC < 450.0){
+ vx = (int)((RIGHT_SONIC - 450.0) / 15.0 - 10.0);
+ if(vx < -30)vx = -30;
+ }else if(LEFT_SONIC < 450.0){
+ vx = (int)((450.0 - LEFT_SONIC ) / 15.0 + 10.0);
+ if(vx > 30)vx = 30;
+ }
+ }else if((RIGHT_SONIC + LEFT_SONIC) <= 600.0){
+ if(BACK_SONIC < 100.0){
+ if(RIGHT_SONIC > LEFT_SONIC){
+ vx = 5;
+ vy = 10;
+ }else{
+ vx = -5;
+ vy = 10;
+ }
+ }else if(BACK_SONIC < 200.0){
+ if(RIGHT_SONIC > LEFT_SONIC){
+ vx = 20;
+ vy = 0;
+ }else{
+ vx = -20;
+ vy = 0;
+ }
+ }
+ }else{
+ if(BACK_SONIC > 500.0){
+ if(RIGHT_SONIC > LEFT_SONIC){
+ vx = 25;
+ vy = -10;
+ }else{
+ vx = -25;
+ vy = -10;
+ }
+ }
+ }
+ }else if(state == DIFFENCE){
+ if(direction == 6){
+ comp_flag10 = 0;
+ if(comp_flag6){
+ if(BACK_SONIC == PING_ERROR){
+
+ }else if(BACK_SONIC > 50.0){
+ vy = -10;
+ }else{
+
+ }
+
+ if(LEFT_SONIC < 500.0){
+ vy = -20;
+ }else if(LEFT_SONIC == PING_ERROR){
+ vx = 20;
+ }else{
+ vx = 20;
+ vy = -20;
+ }
+ }else{
+ if(BACK_SONIC < 110.0){
+ vy = 5;
+ }else if(BACK_SONIC > 500.0){
+ vy = -20;
+ }else if(BACK_SONIC > 300.0){
+ vy = -10;
+ }else if(BACK_SONIC > 140.0){
+ vy = -5;
+ }else{
+ vy = 0;
+ }
+
+ if(LEFT_SONIC < 700.0){
+ vx = 20;
+ }else{
+ if((RIGHT_SONIC < 150.0) || (RIGHT_SONIC == PING_ERROR)){
+ comp_flag6 = 1;
+ }else{
+ vx = 5;
+ vy = -15;
+ }
+ }
+ }
+ }else if(direction == 10){
+ comp_flag6 = 0;
+ if(comp_flag10){
+ if(BACK_SONIC == PING_ERROR){
+
+ }else if(BACK_SONIC > 50.0){
+ vy = -10;
+ }else{
+
+ }
+
+ if(RIGHT_SONIC < 500.0){
+ vy = -20;
+ }else if(RIGHT_SONIC == PING_ERROR){
+ vx = -20;
+ }else{
+ vx = -20;
+ vy = -20;
+ }
+ }else{
+
+ if(BACK_SONIC < 110.0){
+ vy = 5;
+ }else if(BACK_SONIC > 500.0){
+ vy = -20;
+ }else if(BACK_SONIC > 300.0){
+ vy = -10;
+ }else if(BACK_SONIC > 140.0){
+ vy = -5;
+ }else{
+ vy = 0;
+ }
+
+ if(RIGHT_SONIC < 700.0){
+ vx = -20;
+ }else{
+ if((LEFT_SONIC < 150.0) || (LEFT_SONIC == PING_ERROR)){
+ comp_flag10 = 1;
+ }else{
+ vx = -5;
+ vy = -15;
+ }
+ }
+ }
+
+ }else if(direction == 4){
+ comp_flag10 = 0;
+ if(comp_flag6){
+ if(BACK_SONIC == PING_ERROR){
+
+ }else if(BACK_SONIC > 50.0){
+ vy = -10;
+ }else{
+
+ }
+
+ if(LEFT_SONIC < 500.0){
+ vy = -20;
+ }else if(LEFT_SONIC == PING_ERROR){
+ vx = 20;
+ }else{
+ vx = 20;
+ vy = -20;
+ }
+ }else{
+ if(BACK_SONIC < 110.0){
+ vy = 5;
+ }else if(BACK_SONIC > 500.0){
+ vy = -20;
+ }else if(BACK_SONIC > 300.0){
+ vy = -10;
+ }else if(BACK_SONIC > 140.0){
+ vy = -5;
+ }else{
+ vy = 0;
+ }
+
+ if(LEFT_SONIC < 700.0){
+ vx = 20;
+ }else{
+ if((RIGHT_SONIC < 150.0) || (RIGHT_SONIC == PING_ERROR)){
+ comp_flag6 = 1;
+ }else{
+ vx = 5;
+ vy = -15;
+ }
+ }
+ }
+ }else if(direction == 12){
+ comp_flag6 = 0;
+ if(comp_flag10){
+ if(BACK_SONIC == PING_ERROR){
+
+ }else if(BACK_SONIC > 50.0){
+ vy = -10;
+ }else{
+
+ }
+
+ if(RIGHT_SONIC < 500.0){
+ vy = -20;
+ }else if(RIGHT_SONIC == PING_ERROR){
+ vx = -20;
+ }else{
+ vx = -20;
+ vy = -20;
+ }
+ }else{
+ if(BACK_SONIC < 110.0){
+ vy = 5;
+ }else if(BACK_SONIC > 500.0){
+ vy = -20;
+ }else if(BACK_SONIC > 300.0){
+ vy = -10;
+ }else if(BACK_SONIC > 140.0){
+ vy = -5;
+ }else{
+ vy = 0;
+ }
+
+ if(RIGHT_SONIC < 700.0){
+ vx = -20;
+ }else{
+ if((LEFT_SONIC < 150.0) || (LEFT_SONIC == PING_ERROR)){
+ comp_flag10 = 1;
+ }else{
+ vx = -5;
+ vy = -15;
+ }
+ }
+ }
+ }else if(direction == 8){
+ if(comp_flag6){
+ vx = -15;
+ vy = -20;
+ }else if(comp_flag10){
+ vx = 15;
+ vy = -20;
+ }else{
+ if(LEFT_SONIC > RIGHT_SONIC){
+ vx = -15;
+ vy = -20;
+ }else{
+ vx = 15;
+ vy = -20;
+ }
+ }
+ }else if((direction == 2) || (direction == 1)){
+ comp_flag6 = 0;
+ comp_flag10 = 0;
+ if((RIGHT_SONIC > 700.0) && (!(RIGHT_SONIC == PING_ERROR))){
+ vx = 30;
+ }else{
+ vx = 20;
+ }
+ if(BACK_SONIC > 140.0){
+ vy = -10;
+ }else if(BACK_SONIC < 110.0){
+ vy = 5;
+ }
+ }else if((direction == 14) || (direction == 15)){
+ comp_flag6 = 0;
+ comp_flag10 = 0;
+ if((LEFT_SONIC > 700.0) && (!(LEFT_SONIC == PING_ERROR))){
+ vx = -30;
+ }else{
+ vx = -20;
+ }
+ if(BACK_SONIC > 140.0){
+ vy = -10;
+ }else if(BACK_SONIC < 110.0){
+ vy = 5;
+ }
+ }else if(direction == 0){
+ comp_flag6 = 0;
+ comp_flag10 = 0;
+ vx = 0;
+ vy = 20;
+ }else{
+ comp_flag6 = 0;
+ comp_flag10 = 0;
+ vx = 0;
+ vy = 0;
+ }
+ }else if(state == WARNING){
+ comp_flag6 = 0;
+ comp_flag10 = 0;
+ if(direction == 0){
+ vx = 0;
+ }else if(direction == 1){
+ vx = 15;
+ }else if(direction == 2){
+ vx = 20;
+ }else if(direction == 14){
+ vx = -20;
+ }else if(direction == 15){
+ vx = -15;
+ }
+
+ if((LEFT_SONIC > 320) && (RIGHT_SONIC > 320)){
+ if(BACK_SONIC > 150.0){
+ vy = -5;
+ }else if(BACK_SONIC < 110.0){
+ vy = 5;
+ }else{
+ vy = 0;
+ }
+ }else{
+ if(BACK_SONIC > 110.0){
+ vy = -5;
+ }else if(BACK_SONIC < 70.0){
+ vy = 5;
+ }else{
+ vy = 0;
+ }
+ }
+ }
+ move(vx,vy,vs);
+
+ }
+}
\ No newline at end of file