kazunori ogasawara
Motor control program for 260deg sky line song(Katsufumi Matsui's)
Revision 0:c082038ebf4d, committed 2014-07-07
- Comitter:
- macht
- Date:
- Mon Jul 07 09:57:32 2014 +0000
- Commit message:
- Motor control program for 350deg sky line song(Katsufumi Matsui)
Changed in this revision
diff -r 000000000000 -r c082038ebf4d QEI.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Mon Jul 07 09:57:32 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/QEI/#5c2ad81551aa
diff -r 000000000000 -r c082038ebf4d TA8429.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TA8429.lib Mon Jul 07 09:57:32 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/macht/code/TA8429/#a56d6efd9c85
diff -r 000000000000 -r c082038ebf4d main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Jul 07 09:57:32 2014 +0000
@@ -0,0 +1,140 @@
+#include "mbed.h"
+#include "QEI.h"
+#include "TA8429.h"
+#define CONTROL_CYCLE 0.05
+#define PWM_FREQUENCY 20000.0
+
+#define DEFAULT_CW_SPEED 8.0
+#define DEFAULT_CCW_SPEED -90.0
+#define MIN_SPEED 8.0
+#define MAX_SPEED 90.0
+#define REVOLVE_NUM 1
+
+#define Kp 0.01
+#define Ki 0.001
+#define Kd 0
+
+DigitalOut myled(LED1);
+TA8429 driver(p21,p22,PWM_FREQUENCY);
+QEI encoder(p18,p24,p23,1024,QEI::X4_ENCODING);
+Serial pc(USBTX,USBRX);
+DigitalOut ledCW(LED1);
+DigitalOut ledCCW(LED2);
+AnalogIn thermalSensor(p17);
+AnalogIn volume(p16);
+Ticker controlTicker;
+Ticker sensingTicker;
+int g_control_flag;
+
+void set_control_flag(){
+ g_control_flag = 1;
+}
+
+float calc_revolve_speed(){
+ float revolve_speed= 0;
+ int current_pulses = encoder.getPulses();
+ static int last_pulses = 0;
+ revolve_speed = (float)(last_pulses-current_pulses)*360.0/((float)CONTROL_CYCLE*4096.0);
+ last_pulses = current_pulses;
+ return revolve_speed;
+}
+
+float PID_control(float desired_revolve_speed,float current_revolve_speed,int reset_flag){
+ float e,output;
+ static float integrate_e = 0;
+ e = desired_revolve_speed-current_revolve_speed;
+ output = Kp*e+Ki*integrate_e;
+ integrate_e += e;
+ if(reset_flag == 1){
+ integrate_e = 0;
+ output = 0;
+ }
+ return output;
+}
+
+void send_temperature(){
+ float temperature;
+ temperature = (thermalSensor*3.3-0.6)*100.0;
+ pc.printf("%f\r",temperature);
+}
+
+
+float convert_volume2speed(float min,float max){
+ float speed;
+ speed = (max-min)*volume.read()+min;
+ return speed;
+}
+
+int main(){
+ float CW_speed = DEFAULT_CW_SPEED;
+ float CCW_speed = DEFAULT_CCW_SPEED;
+ int revolve_num = REVOLVE_NUM;
+ int mode = 0;
+ float desired_revolve_speed = 0.0;
+ float current_revolve_speed = 0.0;
+ float output = 0.0;
+ char received_char;
+ pc.baud(115200);
+ controlTicker.attach(&set_control_flag,CONTROL_CYCLE);
+ sensingTicker.attach(&send_temperature,0.06);
+ desired_revolve_speed = 10.0;
+ while(1){
+ if(pc.readable()){
+ received_char = pc.getc();
+ switch(received_char){
+ case 'p':
+ driver.stop();
+ controlTicker.detach();
+ myled = 0;
+ break;
+ case 's':
+ controlTicker.attach(&set_control_flag,CONTROL_CYCLE);
+ myled = 1;
+ break;
+ }
+
+ }
+ if(g_control_flag == 1){
+ switch(mode){
+ case 0:
+ desired_revolve_speed = CW_speed;
+ if(encoder.getPulses() < -4096*revolve_num){
+ mode = 1;
+ }
+ break;
+ case 1:
+ driver.stop();
+ PID_control(0,0,1);
+ wait(1);
+ mode = 2;
+ break;
+ case 2:
+ desired_revolve_speed = CCW_speed;
+ if(encoder.getPulses() > 0){
+ mode = 3;
+ }
+ break;
+ case 3:
+ driver.stop();
+ PID_control(0,0,1);
+ wait(1);
+ CW_speed = convert_volume2speed(MIN_SPEED,MAX_SPEED);
+ mode = 0;
+ break;
+ }
+ current_revolve_speed = calc_revolve_speed();
+ output = PID_control(desired_revolve_speed,current_revolve_speed,0);
+ if(output > 0){
+ driver.set_CW(output);
+ }else if(output < 0){
+ driver.set_CCW(-output);
+ }else{
+ driver.stop();
+ }
+ //pc.printf("%f\r",read_sensor());
+ //pc.printf("desired:%.2f current:%.2f output:%.2f\r\n",desired_revolve_speed,current_revolve_speed,output);
+ g_control_flag = 0;
+ }
+ }
+ }
+
diff -r 000000000000 -r c082038ebf4d mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Jul 07 09:57:32 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/8e73be2a2ac1 \ No newline at end of file