ryo seki
test
Dependencies: HMC6352 PID mbed
Diff: main.cpp
- Revision:
- 1:89408fff7cc9
- Parent:
- 0:74bf4953c0d1
--- a/main.cpp Sat Mar 09 10:11:06 2013 +0000
+++ b/main.cpp Sun Mar 10 07:31:31 2013 +0000
@@ -6,6 +6,44 @@
#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("%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("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());
+}
+
/*********** Serial interrupt ***********/
void Tx_interrupt()
@@ -26,67 +64,6 @@
/*********** 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()
{
@@ -114,6 +91,7 @@
}
standard = compass.sample() / 10.0;
+
led1 = OFF;
led3 = OFF;
@@ -146,34 +124,27 @@
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]);
+ vx = (int)(70*ball_sankaku[direction][0]);
+ vy = (int)(70*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;
+ vx = (int)(70*ball_sankaku[direction][0]);
+ vy = (int)(70*ball_sankaku[direction][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){
@@ -183,7 +154,7 @@
vx = -15;
vy = 0;
}else{
- if(ultrasonicVal[1] > 700){
+ if(ultrasonicVal[1] > 800){
vx = 0;
vy = -15;
}else{
@@ -196,19 +167,61 @@
vy = 0;
}
}else if(state == DIFFENCE){
- if((direction == 1) || (direction == 2) || (direction == 3) || (direction == 4) || (direction == 5) || (direction == 6) || (direction == 7)){
- vx = 15;
+ if(ultrasonicVal[1] > 800){
+ vx = 0;
+ vy = -15;
+ }else{
+ if(distance <= 30){
+
+ 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 if((direction == 9) || (direction == 10) || (direction == 11) || (direction == 12) || (direction == 13) || (direction == 14) || (direction == 15)){
+ vx = -15;
+ }else{
+ vx = 0;
+ }
+ }else{
+
+ }
+ }
+ }
+ */
+ /*
+ if(state == HOME_WAIT){
+
+ }*/
+
+ if((ultrasonicVal[0] + ultrasonicVal[2]) < 1050.0){
+ if(ultrasonicVal[0] > 300.0){
+ vx = 15;
+ led3 = ON;
+ led4 = OFF;
+ }else if(ultrasonicVal[2] > 300.0){
+ vx = -15;
+ led3 = ON;
+ led4 = OFF;
+ }else{
+ led3 = OFF;
+ led4 = ON;
+ if(ultrasonicVal[1] > 200.0){
+ vy = -15;
+ }else if(ultrasonicVal[1] < 100.0){
+ vy = 15;
+ }else{
+ vy = 0;
+ }
+ }
+ led2 = ON;
}else{
vx = 0;
-
+ vy = 0;
+ led2 = OFF;
}
- }
- */
-
+
+ //pc.printf("%f,%f,%f\n",ultrasonicVal[0],ultrasonicVal[1],ultrasonicVal[2]);
+
move(vx,vy,vs);
}
}