Revision 23:b79faf58d4a1, committed 2015-06-01

Comitter:

heuristics

Date:

Mon Jun 01 06:06:28 2015 +0000

Parent:

22:5cad60d669e6

Child:

24:fa52fd530d6e

Commit message:

fgkol

Changed in this revision

--- a/MotorControl.lib	Tue May 26 10:22:47 2015 +0000
+++ b/MotorControl.lib	Mon Jun 01 06:06:28 2015 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/teams/Cool-step/code/MotorControl/#5df3b4a9a368
+http://developer.mbed.org/teams/Cool-step/code/MotorControl/#d950f0c95da1

--- a/main.cpp	Tue May 26 10:22:47 2015 +0000
+++ b/main.cpp	Mon Jun 01 06:06:28 2015 +0000
@@ -11,6 +11,7 @@
 DigitalOut led2(P2_7);
 DigitalOut led3(P2_8);
 AnalogIn hallSensor(P0_26);
+float P=0,I=0,D=0;
 int pulsesMotor2=0,pulsesMotor1=0;
 int int_time=0,bint_time=0;
 //InterruptIn hall1_(P0_5), hall2_(P0_4);
@@ -19,7 +20,7 @@
 Serial RN42(P0_10, P0_11);
 Serial debug(P0_2, P0_3);
 
-Ticker infoTicker,commandTicker;
+Ticker infoTicker,commandTicker,swaveTicker;
 
 Timer timer;
 Timer timer2;
@@ -47,6 +48,18 @@
     }
     return num;
 }
+
+bool squarewave=0;
+void swaveTask(void)
+{
+    if(squarewave) {
+        motor2.setAngle(0);
+    } else {
+        motor2.setAngle(60);
+    }
+    squarewave=!squarewave;
+}
+float pidStep=1000.0f;
 void getCommand(void)
 {
     if(debug.readable()) {
@@ -63,8 +76,34 @@
 
                 motor2.setAngle(speed);
             }
+        } else if(buffer[0]=='p') {
+            if(buffer[1]=='+') {
+                P+=1/pidStep;
+            }
 
+            if(buffer[1]=='-') {
+                P-=1/pidStep;
+            }
+            motor2.setPID(P,I,D);
+        } else if(buffer[0]=='i') {
+            if(buffer[1]=='+') {
+                I+=1/pidStep;
+            }
+            if(buffer[1]=='-') {
+                I-=1/pidStep;
+            }
+            motor2.setPID(P,I,D);
+        } else if(buffer[0]=='d') {
+            if(buffer[1]=='+') {
+                D+=1/pidStep;
+            }
 
+            if(buffer[1]=='-') {
+                D-=1/pidStep;
+            }
+            motor2.setPID(P,I,D);
+        } else if(buffer[0]=='s') {
+            pidStep = (float)(str2int(buffer+2));
         }
     }
 }
@@ -76,8 +115,8 @@
     //InitTimer0();
     //initialize_connection();
     infoTicker.attach(infoTask,0.2f);
-    commandTicker.attach(getCommand,0.2f);
-
+   // commandTicker.attach(getCommand,0.2f);
+    swaveTicker.attach(swaveTask,1.0f);
     if(imu.isReady()) {
         debug.printf("MPU9150 is ready\r\n");
     } else {
@@ -96,13 +135,11 @@
     Quaternion q1;
     wait_ms(500);
     motor1.setAngle(0);
-    
+
     motor2.setAngle(0);
     initCaptures();
-    motor1.setAngle(0);
-    motor2.setAngle(0);
-    
-    
+
+
     while(1) {
         if(imu.getFifoCount() >= 48) {
             imu.getFifoBuffer(buffer,  48);
@@ -152,23 +189,24 @@
             debug.printf("p: %f r: %f y: %f ",pry.x/PI*180,pry.y/PI*180,pry.z/PI*180);
             debug.printf("m1: %.2f m2: %.2f ",motor1.getAngle(),motor2.getAngle());
             debug.printf("dt1: %f dt2: %f ",motor1.get_dt(),motor2.get_dt());
-            debug.printf("Direction: %d Int Time: %d  \r\n ",bint_time,int_time);
-           /* if(!motor1.isRunning()) {
-                if(motor1.getAngle()>30) {
-                    motor1.setAngle(0);
-                }
-                if(motor1.getAngle()<30) {
-                    motor1.setAngle(60);
-                }
-            }
-            if(!motor2.isRunning()) {
-                if(motor2.getAngle()>30) {
-                    motor2.setAngle(0);
-                }
-                if(motor2.getAngle()<30) {
-                    motor2.setAngle(60);
-                }
-            }*/
+            debug.printf("P: %f I: %f I: %f  \r\n ",P,I,D);
+            getCommand();
+            /*  if(!motor1.isRunning()) {
+                  if(motor1.getAngle()>30) {
+                      motor1.setAngle(0);
+                  }
+                  if(motor1.getAngle()<30) {
+                      motor1.setAngle(60);
+                  }
+              }
+              if(!motor2.isRunning()) {
+                  if(motor2.getAngle()>30) {
+                      motor2.setAngle(0);
+                  }
+                  if(motor2.getAngle()<30) {
+                      motor2.setAngle(60);
+                  }
+              }*/
         }
     }
 
@@ -177,47 +215,9 @@
 
 
 
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
 void TIMER2_IRQHandler(void)
 {
-         //   timer2.reset();
+    //   timer2.reset();
     int bitB = (LPC_GPIO0->FIOPIN>>5) & 0x1;
     if(bitB) {
         pulsesMotor2--;