Revision 2:53a942d1b1e5, committed 2016-04-22

Comitter:

adam_z

Date:

Fri Apr 22 09:39:42 2016 +0000

Parent:

1:ddc39900f9b8

Child:

3:b0a66fde7dc3

Commit message:

start integrating current control lib;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CURRENT_CONTROL.lib	Fri Apr 22 09:39:42 2016 +0000
@@ -0,0 +1,1 @@
+CURRENT_CONTROL#955aa05c968a

--- a/PID.lib	Thu Apr 21 08:50:02 2016 +0000
+++ b/PID.lib	Fri Apr 22 09:39:42 2016 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/teams/LDSC_Robotics_TAs/code/PID/#7f9b4ca968ae
+http://developer.mbed.org/teams/LDSC_Robotics_TAs/code/PID/#4df4895863cd

--- a/main.cpp	Thu Apr 21 08:50:02 2016 +0000
+++ b/main.cpp	Fri Apr 22 09:39:42 2016 +0000
@@ -4,6 +4,7 @@
 #include "QEI.h"
 
 #define Ts 0.001
+#define pi 3.14159
 
 LSM9DS0 sensor(SPI_MODE, D9, D6);
 Serial pc(SERIAL_TX, SERIAL_RX);
@@ -16,12 +17,28 @@
 PwmOut M2C1(D8);
 PwmOut M2C2(A3);
 
-DigitalOut led1(LED1);
+QEI wheel_L(A1, A2, NC, 280, 50, 0.001, QEI::X4_ENCODING);
+QEI wheel_R(D13, D12, NC, 280, 50, 0.001, QEI::X4_ENCODING);
+
+AnalogIn current_L(PC_2);
+AnalogIn current_R(PC_3);
+
+PID curL_PID(0.6,2,0.0,0.001);
+
+
+int tim_counter;
+float current_L_Offset;
+float current_R_Offset;
+float tim = 0.0;
+float amp = 0.4;
+float omega = 2.0;
+float curCmd_L;
+
 
 int main()
 {
     //float abias[3], gbias[3];
-    
+
     pc.baud(115200);
     if( sensor.begin() != 0 ) {
         pc.printf("Problem starting the sensor with CS @ Pin D6.\r\n");
@@ -31,33 +48,47 @@
     //sensor.calLSM9DS0(gbias, abias);
     sensor.setGyroOffset(38,-24,-106);
     sensor.setAccelOffset(-793,-511,300);
-    
+
     M1C1.period_us(50);
-    M1C1.write(0.7);
+    M1C1.write(0.5);
     M2C1.period_us(50);
-    M2C1.write(0.7);
+    M2C1.write(0.5);
     TIM1->CCER |= 4; //enable ch1 complimentary output
     TIM1->CCER |= 64;//enable ch1 complimentary output
-    
-    
+
+
+
     WIPVTimer.attach_us(WIPVTimerInterrupt, 1000.0);
-    while(true) {}
+    while(true) {
+        //pc.printf("%5f\t%5f\r\n", speed1.getAngularSpeed(), speed2.getAngularSpeed());
+    }
 }
 
 
 void WIPVTimerInterrupt()
 {
-    
+    if(tim_counter <100)tim_counter++;
+    else if (tim_counter >= 100 && tim_counter <=109) {
+        current_L_Offset += current_L.read();
+        current_R_Offset += current_R.read();
+        tim_counter++;
+        if(tim_counter == 110) {
+            current_L_Offset = current_L_Offset/10;
+            current_R_Offset = current_R_Offset/10;
+        }
+
+    } else {
+
         /*
         int16_t data_array[6];
-        
+
         data_array[0]  = sensor.readRawAccelX();
         data_array[1]  = sensor.readRawAccelY();
         data_array[2]  = sensor.readRawAccelZ();
         data_array[3]  = sensor.readRawGyroX();
         data_array[4]  = sensor.readRawGyroY();
         data_array[5]  = sensor.readRawGyroZ();
-        
+
         pc.printf("%d, ", data_array[0]);
         pc.printf("%d, ", data_array[1]);
         pc.printf("%d, ", data_array[2]);
@@ -65,8 +96,8 @@
         pc.printf("%d, ", data_array[4]);
         pc.printf("%d;\r\n ", data_array[5]);
         */
-        
-        
+
+
         float data_array[6];
         data_array[0]  = sensor.readFloatAccelX();
         data_array[1]  = sensor.readFloatAccelY();
@@ -75,18 +106,38 @@
         data_array[4]  = sensor.readFloatGyroY();
         data_array[5]  = sensor.readFloatGyroZ();
         sensor.complementaryFilter(data_array, Ts);
+        /*
+                pc.printf("%5.4f, ", sensor.pitch);
+                pc.printf("%5.4f   ", sensor.roll);
+                pc.printf("%5.4f, ", data_array[0]);
+                pc.printf("%5.4f, ", data_array[1]);
+                pc.printf("%5.4f    ", data_array[2]);
+                pc.printf("%5.4f, ", data_array[3]);
+                pc.printf("%5.4f, ", data_array[4]);
+                pc.printf("%5.4f;\r\n", data_array[5]);
+         */
+        //*****wheel speed calculation*****//
+        wheel_L.Calculate();
+        wheel_R.Calculate();
+        //*************obtain current values***********//
+        //pc.printf("%f\t", current_L_Offset);
+        //pc.printf("%f\r\n", current_R_Offset);
+        //*************current control********//
+        tim += Ts;
+        if(tim >= 4*pi/omega)tim = 0.0;
+        curCmd_L = amp*sin(omega*tim); //current command
         
-        pc.printf("%5.4f, ", sensor.pitch);
-        pc.printf("%5.4f   ", sensor.roll);
-        pc.printf("%5.4f, ", data_array[0]);
-        pc.printf("%5.4f, ", data_array[1]);
-        pc.printf("%5.4f    ", data_array[2]);
-        pc.printf("%5.4f, ", data_array[3]);
-        pc.printf("%5.4f, ", data_array[4]);
-        pc.printf("%5.4f;\r\n", data_array[5]);
+        pc.printf("%5.4f\t", 10*curCmd_L);
+        float cur_L = (current_L.read() - current_L_Offset)*3.3*8/0.6; //8A when the voltage is 0.6 (assuming linear)
+        pc.printf("%5.4f\t", 10*cur_L);
         
-        
+        curL_PID.Compute(curCmd_L, cur_L);
+        M1C1 = 0.5 - curL_PID.output;
+        pc.printf("%5.4f\r\n", curL_PID.output);
+        TIM1->CCER |= 4;
         
-        led1 = !led1;
-        
+
+    }
+
+
 }
\ No newline at end of file