Diff: inits.h

Revision:

10:e807253d4879

Child:

12:d7bb475bb82d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/inits.h	Tue Oct 06 10:19:15 2015 +0000
@@ -0,0 +1,82 @@
+//****************************************************************************/
+// Defines
+//****************************************************************************/
+#define RATE  0.01 
+#define Kc    0.30
+#define Ti    0.15
+#define Td    0.0
+
+//****************************************************************************/
+// Globals
+//****************************************************************************/
+Serial pc(USBTX, USBRX);
+HIDScope    scope(3);        // Instantize a 2-channel HIDScope object
+Ticker      scopeTimer;      // Instantize the timer for sending data to the PC 
+//--------
+// Motors 
+//--------
+// Left motor.
+PwmOut leftMotor(D5);
+DigitalOut leftDirection(D4);
+QEI leftQei(D12, D13, NC, 624);
+PID leftController(Kc, Ti, Td, RATE);
+
+// Right motor
+PwmOut rightMotor(D6);
+DigitalOut rightDirection(D7);
+QEI rightQei(D10, D11, NC, 624);
+PID rightController(Kc, Ti, Td, RATE);
+
+// EMG input
+AnalogIn pot1(A0);
+AnalogIn pot2(A1);
+
+
+// Timers
+Timer endTimer;
+Ticker motorControlTicker;
+bool goFlag=false;
+
+// Working variables.
+volatile int leftPulses     = 0;
+volatile int leftPrevPulses = 0;
+volatile float leftPwmDuty  = 0.0;
+volatile float leftPwmDutyPrev = 0.0;
+volatile float leftVelocity = 0.0;
+
+volatile int rightPulses     = 0;
+volatile int rightPrevPulses = 0;
+volatile float rightPwmDuty  = 0.0;
+volatile float rightPwmDutyPrev = 0.0;
+volatile float rightVelocity = 0.0;
+
+float request;
+float request_pos;
+float request_neg;
+
+void initMotors(){
+    //Initialization of motor
+    leftMotor.period_us(50);
+    leftMotor = 0.0;
+    leftDirection = 1;
+    
+    rightMotor.period_us(50);
+    rightMotor = 0.0;
+    rightDirection = 1;
+    
+}
+
+void initPIDs(){
+    //Initialization of PID controller
+    leftController.setInputLimits(-1.0, 1.0);
+    leftController.setOutputLimits(-1.0, 1.0);
+    leftController.setBias(0);
+    leftController.setDeadzone(-0.4, 0.4);
+    leftController.setMode(AUTO_MODE);
+    
+    rightController.setInputLimits(-1.0, 1.0);
+    rightController.setOutputLimits(-1.0, 1.0);
+    rightController.setBias(0);
+    rightController.setDeadzone(-0.4, 0.4);
+    rightController.setMode(AUTO_MODE);
+}
\ No newline at end of file