yusuke takahashi
aaaaa
Dependencies: HMC6352 PID QEI Servo mbed
Revision 0:1be472d79ae9, committed 2013-09-30
- Comitter:
- yusuke_robocup
- Date:
- Mon Sep 30 09:01:37 2013 +0000
- Commit message:
- PIDsync
Changed in this revision
diff -r 000000000000 -r 1be472d79ae9 HMC6352.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/HMC6352.lib Mon Sep 30 09:01:37 2013 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/aberk/code/HMC6352/#83c0cb554099
diff -r 000000000000 -r 1be472d79ae9 PID.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.lib Mon Sep 30 09:01:37 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/PID/#6e12a3e5af19
diff -r 000000000000 -r 1be472d79ae9 QEI.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Mon Sep 30 09:01:37 2013 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/aberk/code/QEI/#5c2ad81551aa
diff -r 000000000000 -r 1be472d79ae9 Servo.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo.lib Mon Sep 30 09:01:37 2013 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/simon/code/Servo/#36b69a7ced07
diff -r 000000000000 -r 1be472d79ae9 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Sep 30 09:01:37 2013 +0000
@@ -0,0 +1,293 @@
+#include "mbed.h"
+#include "PID.h"
+#include "QEI.h"
+#include "Servo.h"
+#include "HMC6352.h"
+#include "main.h"
+
+//guro-baru
+double vl = 0;
+double vc = 0;
+double vs = 1;
+int clwise = 0;
+int anclwise = 0;
+
+int enkoda1;
+int enkoda2;
+
+int process = 0;
+int vc_count = 0;
+int mode_comp = 0;
+
+int inputPID = 0;
+
+//guro-baru end
+
+
+void PidUpdate(){
+ static int state = WAIT,past_state = WAIT,next_state = WAIT;
+
+ if(!mode_comp){
+ if(vl && !vs){
+ state = STRAIGHT;
+ }
+ if(vs){
+ state = TURN;
+ }
+ }
+ if((state != past_state)){
+ mode_comp = 1;
+
+ if(process == 0){
+ if(abs(enkoda1) > 180 && abs(enkoda1) < 240){
+ process++;
+ }
+ }
+ if(process == 1){
+ if(state == STRAIGHT){
+ vl = 10;
+ vs = 0;
+ }
+ if(state == TURN){
+ vl = 0;
+ vs = 10;
+ }
+ if(abs(vc) < 60){
+ vc_count++;
+ }
+ if(vc_count > 2){
+ vc_count = 0;
+ mode_comp = 0;
+ process = 0;
+ }
+ }
+ }
+
+ enkoda1 = (int)(((double)wheel1.getPulses()/CYCLE) * 360.0);
+ enkoda2 = (int)(((double)wheel2.getPulses()/CYCLE) * 360.0);
+
+ if((!vs)&&(mode_comp == 0)){
+ pid.setProcessValue(wheel1.getPulses() + wheel2.getPulses());
+ }else if((vs)&&(mode_comp == 0)){
+ pid.setProcessValue(wheel1.getPulses() - wheel2.getPulses());
+ }
+
+ if((!vs)&&(mode_comp)){
+ pid.setProcessValue(wheel1.getPulses() + wheel2.getPulses());
+ }else if((vs)&&(mode_comp)){
+ pid.setProcessValue(wheel1.getPulses() - wheel2.getPulses());
+ }
+
+ vc = (int)pid.compute();
+
+ printf("vc:%lf\n",vc);
+
+ double fut_R = 0.5,fut_L = 0.5;
+
+ if(abs(vc) > 250){
+ vc = 0;
+ }
+
+ int vlR = 0;
+ int vlL = 0;
+ int vcR = vc;
+ int vcL = vc;
+
+ vlR = -vs;
+ vlL = vs;
+
+ vl *= 0.5;
+ vc *= 0.5;
+
+ vlR *= 0.4;
+ vlL *= 0.4;
+
+ vcR *= 0.6;
+ vcL *= 0.6;
+
+
+ if(!vs){
+ if(vl > 0){
+ fut_R = Convert_dekaruto((int)(vl - vc));
+ fut_L = Convert_dekaruto((int)(vl * 0.95 + vc));
+ }
+ if(vl < 0){
+ vc *= -1;
+ fut_R = Convert_dekaruto((int)(vl * 0.95 + vc));
+ fut_L = Convert_dekaruto((int)(vl - vc));
+ }
+ }else{
+ if(vlR < 0){
+ vcR *= -1;
+ }
+
+ if(vlL < 0){
+ vcL *= -1;
+ }
+ if(vs > 0){
+ fut_R = Convert_dekaruto((int)(vlR * 0.9 + vcR));
+ fut_L = Convert_dekaruto((int)(vlL * 0.65 - vcL));
+ }
+
+ if(vs < 0){
+ fut_R = Convert_dekaruto((int)(vlR * 0.65 - vcR));
+ fut_L = Convert_dekaruto((int)(vlL * 0.9+ vcL));
+ }
+ }
+
+ servoR = fut_R;
+ servoL = fut_L;
+
+ if(!mode_comp){
+ past_state = state;
+ }
+}
+
+void CompassUpdate(){
+ inputPID = (((int)(compass.sample() - (/**/180 * 10.0) + 5400.0) % 3600) / 10.0);
+}
+
+double vssOut(){
+ double vss;
+ vss = ((inputPID / 360.0 - 0.5) * 2.0 * 1000.0) * 1.0;
+
+ if(abs(vss) < 10){
+ vss = 0;
+ }
+
+ vss *= 3.0;
+
+
+ if((vss)&&(abs(vss) < 500)){
+ vss += (vss/abs(vss)) * 500;
+ }
+
+ if(abs(vss) > 1000){
+ vss = 1000 * (vss/abs(vss));
+ }
+
+ return vss;
+}
+
+void move(int vll,int vss){
+ if(!swR){
+ wheel1.reset();
+ }
+
+ if(!swL){
+ wheel2.reset();
+ }
+
+ vl = vll;
+ vs = vss;
+}
+
+#define PINR_THR 2000
+
+int ping_button(int ping,int button){
+ static int continue_flag = 0;
+ static int change_flag = 0;
+
+ if(continue_flag == 0){
+ if(ping <= PINR_THR){
+ ping_t.start();
+ continue_flag = 1;
+ }
+ }
+
+ if(continue_flag == 1){
+ //agatterutoki
+ if(ping <= PINR_THR){
+ if(change_flag == 0){
+ if(ping_t.read_ms() >= 150){
+ button = !button;
+ change_flag = 1;
+ }
+ }
+ }
+ //tatisagari
+ if(ping >= (PINR_THR+200)){
+ ping_t.stop();
+ ping_t.reset();
+ continue_flag = 0;
+ change_flag = 0;
+ }
+ }
+ return button;
+}
+
+
+int main() {
+ wait(3);
+
+ timer2.start();
+ ping_t.start();
+
+ pid.setInputLimits(MINIMUM,MAXIMUM);
+ pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT);
+ pid.setBias(PID_BIAS);
+ pid.setMode(AUTO_MODE);
+ pid.setSetPoint(REFERENCE);
+
+ compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+ compassUpdata.attach(&CompassUpdate, COMPASS_CYCLE);
+
+ swR.mode(PullUp);
+ swL.mode(PullUp);
+
+ int setR = 0,setL = 0;
+ int vll = 0,vss = 0;
+
+ servoR = 0.595;
+ servoL = 0.59;
+
+ while(1){
+ if(!swR){
+ setR = 1;
+ }
+
+ if(!swL){
+ setL = 1;
+ }
+
+ if(setR){
+ servoR = 0.5;
+ }
+
+ if(setL){
+ servoL = 0.5;
+ }
+
+ if(setR && setL){
+ break;
+ }
+
+ wait(0.1);
+ }
+
+ wheel1.reset();
+ wheel2.reset();
+
+ pidUpdata.attach(&PidUpdate, PID_CYCLE);
+
+ int button = 0;
+
+ while(1) {
+ vll = 0;
+ vss = 0;
+
+ Ultrasonic();
+
+ button = ping_button(ultrasonicVal[0],button);
+
+ if(button){
+ vll = 400;
+ led1 = 1;
+ }else{
+ vll = -500;
+ led1 = 0;
+ }
+
+ move(vll,vss);
+ }
+}
diff -r 000000000000 -r 1be472d79ae9 main.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.h Mon Sep 30 09:01:37 2013 +0000
@@ -0,0 +1,50 @@
+#define RATE 0.1
+#define MINIMUM -1440
+#define MAXIMUM 1440
+
+#define PID_CYCLE 0.03 //s
+#define COMPASS_CYCLE 0.1
+
+#define P_GAIN 1.2 //0.78 //zensin 1.5
+#define I_GAIN 1000000000.0
+#define D_GAIN 0.0 //0.009
+
+#define PID_BIAS 0.0
+#define REFERENCE 0
+
+#define OUT_LIMIT 1000
+
+extern double ultrasonicValue[4];
+extern uint16_t ultrasonicVal[4];
+extern void Ultrasonic(void);
+Timer timer2;
+Timer ping_t;
+
+HMC6352 compass(p28/*sda*/, p27/*scl*/);
+Ticker compassUpdata;
+
+PID pid(P_GAIN,I_GAIN,D_GAIN, RATE);
+PID controller(1.0, 0.0, 0.0, RATE);
+Ticker pidUpdata;
+
+Servo servoR(p23);
+Servo servoL(p24);
+
+DigitalIn swR(p29);
+DigitalIn swL(p30);
+
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+
+QEI wheel1 (p18, p17, NC, 624);
+QEI wheel2 (p8, p7, NC, 624);
+
+#define Convert_dekaruto(a) ((a+1000.0)/2.0/1000.0)
+#define CYCLE 705.0
+
+enum{
+ WAIT,
+ STRAIGHT,
+ TURN,
+ COMP
+};
\ No newline at end of file
diff -r 000000000000 -r 1be472d79ae9 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Sep 30 09:01:37 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/5798e58a58b1 \ No newline at end of file
diff -r 000000000000 -r 1be472d79ae9 ping/ping.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ping/ping.cpp Mon Sep 30 09:01:37 2013 +0000
@@ -0,0 +1,49 @@
+#include "mbed.h"
+#include "ping.h"
+
+//DigitalOut myled = LED1;
+
+extern Timer timer2;
+
+uint16_t ultrasonicVal[ALL_ULTRASONIC];
+double ultrasonicValue[ALL_ULTRASONIC] = {0};
+
+
+void Ultrasonic()
+{
+ for(int 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() > 1500){ //1.5ms以上応答なし
+ ultrasonicValue[i] = PING_ERR;
+ flag = 1;
+ break;
+ }
+ }
+
+ timer2.reset();
+ while(PingPin == 1){
+ if((timer2.read_us() > 18500) || (flag == 1)){ //18.5ms以上のパルス
+ ultrasonicValue[i] = PING_ERR;
+ flag = 1;
+ break;
+ }
+ }
+
+ if(flag == 0){
+ ultrasonicValue[i] = timer2.read_us() / 1000000.0 / 2.0 * 340.0 * 1000.0; //mm MAX:3145
+ ultrasonicVal[i] = (int)(ultrasonicValue[i] * 10.0);
+ }else{
+ ultrasonicVal[i] = PING_ERR;
+ }
+
+ }
+}
\ No newline at end of file
diff -r 000000000000 -r 1be472d79ae9 ping/ping.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ping/ping.h Mon Sep 30 09:01:37 2013 +0000
@@ -0,0 +1,7 @@
+#define ALL_ULTRASONIC 1
+#define PING_ERR 0xFFFF
+
+
+PinName ultrasonic_pin[ALL_ULTRASONIC] = {
+ p21
+};