Diff: main.cpp

Revision:

4:d52e31c73e50

Parent:

3:f169c357a44d

Child:

5:0998ea3869ff

--- a/main.cpp	Tue Sep 03 03:59:03 2013 +0000
+++ b/main.cpp	Thu Nov 19 00:02:49 2015 +0000
@@ -4,7 +4,7 @@
 
 
 //create a new quadrature encoder object
-QEI encoder(p25, p26, NC, 1200, QEI::X4_ENCODING);       //(encoder channel 1 pin, encoder channel 2 pin, index (n/a here), counts per revolution, mode (X4, X2))
+QEI encoder(PTB6, PTB7, NC, 1200, QEI::X4_ENCODING);       //(encoder channel 1 pin, encoder channel 2 pin, index (n/a here), counts per revolution, mode (X4, X2))
 
 //Ticker SampleFrequency;
 
@@ -12,7 +12,7 @@
 
 int steps_per_rev = 1200; //Encoder CPR * gear ratio
 float position = 0;
-float error = 0;
+float p_error = 0;
 float old_error = 0;
 float goal_position = 0;
 float p_gain = 1;
@@ -21,9 +21,9 @@
 float command = 0;
 
 
-PwmOut Motor[2] = {p21, p22};       //h-bridge pwm pins
-DigitalOut dir1[2] = {p29, p15};    //h-bridge direction 1 pins
-DigitalOut dir2[2] = {p30, p16}; 
+PwmOut Motor = PTB13;       //h-bridge pwm pins
+DigitalOut dir1 = PTA0;    //h-bridge direction 1 pins
+DigitalOut dir2 = PTA8; 
 //
 
 float pulsesToRadians(float pulses) //Encoder steps to revolutions of output shaft in radians
@@ -31,42 +31,45 @@
     return ((pulses/steps_per_rev)*(2*pi));
 }
 
-void signal_to_hbridge( float signal, int motor)   //Input of -1 means full reverse, 1 means full forward
+void signal_to_hbridge( float signal)   //Input of -1 means full reverse, 1 means full forward
 {                                       //An input of magnitude > 1 gets reduced to 1 (or -1) by the pwm.write function
     if (signal < 0) {
-        dir1[motor].write(0);
-        dir2[motor].write(1);
-        Motor[motor].write(signal * -1);
+        dir1.write(0);
+        dir2.write(1);
+        Motor.write(signal * -1);
     } 
     else if (signal > 0) {
-        dir1[motor].write(1);
-        dir2[motor].write(0);
-        Motor[motor].write(signal);
+        dir1.write(1);
+        dir2.write(0);
+        Motor.write(signal);
     } 
     else {
-        dir1[motor].write(0);
-        dir2[motor].write(0);
+        dir1.write(0);
+        dir2.write(0);
     }
 }
 float updateCommand(void){
     
     position = pulsesToRadians(encoder.getPulses());        //use the getPulses functin to read encoder position, convert value to radians
-    old_error = error;                                      
-    error = position - goal_position;
-    return p_gain*error + d_gain*(error-old_error)/sample_period;    //modify the command based on position feedback
+    old_error = p_error;                                      
+    p_error = position - goal_position;
+    return p_gain*p_error + d_gain*(p_error-old_error)/sample_period;    //modify the command based on position feedback
     
 }
 
 
 
-
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
 
 int main() {
+    led1 = 0;
+    led2 = 1;
     Timer timer;
     timer.start();
-    Motor[0].period(.00005);
-    Motor[1].period(.00005);
+    Motor.period(.00005);
     //SampleFrequency.attach(&updateCommand, sample_period);        //Encoder will be sampled at 2.5 kHz
+    
     while(1) {
     
            if (timer.read() > .0004){
@@ -74,7 +77,7 @@
                 timer.reset();
 
            }
-           signal_to_hbridge(command, 0);
+           signal_to_hbridge(command);
      
         }
     }