Diff: main.cpp

Revision:

5:e90c8b57811c

Parent:

4:5ae9f8b3a16f

--- a/main.cpp	Tue Nov 24 03:56:22 2015 +0000
+++ b/main.cpp	Mon Dec 24 03:47:49 2018 +0000
@@ -1,20 +1,17 @@
-//Ben Katz, 2013
-//Austin Buchan, 2015
 #include "mbed.h"
 #include "Motor.h"
 #include "PID.h"
 
-#define PWM_PIN PTB13
-#define MIN_1_PIN PTA0
-#define MIN_2_PIN PTA8
-#define ENC_A_PIN PTB6
-#define ENC_B_PIN PTB7
-#define CURRENT_PIN PTA9
+#define PWM_PIN p22
+#define MIN_1_PIN p19
+#define MIN_2_PIN p20
+#define ENC_A_PIN p16
+#define ENC_B_PIN p17
+#define CURRENT_PIN p18
 
-#define UART_RX_PIN PTB1
-#define UART_TX_PIN PTB2
+#define pi 3.141592654
 
-#define PWM_PERIOD 0.001
+#define PWM_PERIOD 0.00005
 #define PID_CUR_PERIOD 0.001
 #define PID_POS_PERIOD 0.005
 
@@ -27,7 +24,8 @@
 //#define PID_CUR_I_GAIN  0.001
 #define PID_CUR_I_GAIN  0.01
 
-Serial pc(UART_TX_PIN, UART_RX_PIN);
+Serial pc(USBTX, USBRX);
+
 
 Motor motor(
     PWM_PIN, MIN_1_PIN, MIN_2_PIN, PWM_PERIOD, 
@@ -36,9 +34,9 @@
     PID_CUR_P_GAIN, PID_CUR_D_GAIN, PID_CUR_I_GAIN
 );
 
-//HBridge hbridge(PWM_PIN, MIN_1_PIN, MIN_2_PIN, PWM_PERIOD);
+HBridge hbridge(PWM_PIN, MIN_1_PIN, MIN_2_PIN, PWM_PERIOD);
 //CurrentSense current_sense(CURRENT_PIN);
-//Encoder encoder(ENC_A_PIN, ENC_B_PIN);
+Encoder encoder(ENC_A_PIN, ENC_B_PIN);
 
 PIDController position_controller(
     PID_POS_P_GAIN, PID_POS_D_GAIN, PID_POS_I_GAIN
@@ -61,44 +59,53 @@
 DigitalOut led3(LED3);
 
 int main() {
-    printf("Built " __DATE__ " " __TIME__ "\r\n");
+    wait(5);
+    pc.printf("Built " __DATE__ " " __TIME__ "\r\n");
     
     led1 = 1;
     led2 = 1;
     led3 = 1;
     
-    wait(.5);
+    wait(5);
     
     led2 = 0;
-    printf("Initializing\n\r");
+    pc.printf("Initializing\n\r");
     
-    current_ticker.attach(&update_current, PID_CUR_PERIOD);
-    position_ticker.attach(&update_position, PID_POS_PERIOD);
+  //  current_ticker.attach(&update_current, PID_CUR_PERIOD);
+   // position_ticker.attach(&update_position, PID_POS_PERIOD);
     
     int count = 0;
     float command = 0.0;
-    
+  //  wait(5);
     led3 = 0;
-    printf("Starting\n\r");
+    pc.printf("Starting\n\r");
+    
+    hbridge.initialize();
     
     while(1){
-        //command = 0.05 * abs((50-count)/10) - 0.1;
-        //motor.set_command(command);
-        
-        command = 1.0 * abs((55-count)/10) - 0.25;
-        position_controller.set_command(command);
+      //command = 0.05 * abs((50-count)/10) - 0.1;
+       //motor.set_command(command);
+       command = sin(count*pi/50);
+        if(command>0)
+        {
+            hbridge.forward();
+        }
+        else if(command<0)
+        {
+             hbridge.back();
+        }
         
         led1 = (count % 2);
+
         
-        //printf("C:%F Cu:%F P:%F\n\r", command, current_sense.get_current(), encoder.get_position());
+        pc.printf("Position:%d\n\r",(int) encoder.get_position());
         //printf("PC:%F\n\r", position_controller.command);
-        printf("MP:%F MV:%F MCu:%F MCo:%F MO:%F MT:%F P:%F\n\r", 
-            motor.position, motor.velocity, motor.current, motor.command, motor.output, motor.torque, position_controller.command
-        );
+        //pc.printf("MP:%F MV:%F MCu:%F MCo:%F MO:%F MT:%F P:%F\n\r", 
+         //   motor.position, motor.velocity, motor.current, motor.command, motor.output, motor.torque, position_controller.command
+       //);
         
         
-        count = (count + 1) % 100;
-        
-        wait(0.1);
+        count++;
+        wait(0.01);
     }
 }
\ No newline at end of file