Diff: inits.h

Revision:

0:ca94aa9bf823

Child:

2:8f9b6c1e89cf

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/inits.h	Tue Oct 13 17:46:45 2015 +0000
@@ -0,0 +1,107 @@
+//****************************************************************************/
+// Defines
+//****************************************************************************/
+#define RATE  0.005 
+#define Kc    0.30
+#define Ti    0.00
+#define Td    0.0
+
+//****************************************************************************/
+// Globals
+//****************************************************************************/
+Serial pc(USBTX, USBRX);
+HIDScope    scope(5);        // Instantize a 2-channel HIDScope object
+Ticker      scopeTimer;      // Instantize the timer for sending data to the PC 
+InterruptIn startButton(D3);
+InterruptIn stopButton(D2);
+//--------
+// 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(D11, D10, NC, 624);
+PID rightController(Kc, Ti, Td, RATE);
+
+// edge leads
+DigitalOut outofboundsled(LED1);
+DigitalIn edgeleft(D0);
+DigitalIn edgeright(D1);
+
+
+// EMG input
+AnalogIn pot1(A0);
+AnalogIn pot2(A1);
+biquadFilter leftVelocityfilter((double) -1.561018075800718, (double) 0.641351538057563, (double) 0.020083365564211, (double) 0.040166731128423, (double) 0.020083365564211);
+biquadFilter rightVelocityfilter((double) -1.561018075800718, (double) 0.641351538057563, (double) 0.020083365564211, (double) 0.040166731128423, (double) 0.020083365564211);
+// Timers
+Timer endTimer;
+Ticker motorControlTicker;
+bool goFlag=false;
+bool systemOn=false;
+// Working variables.
+volatile int leftPulses     = 0;
+volatile float leftAngle=0;
+volatile int leftPrevPulses = 0;
+volatile float leftPwmDuty  = 0.0;
+volatile float leftPwmDutyPrev = 0.0;
+volatile float leftVelocity = 0.0;
+float leftRequest=0;
+
+volatile int rightPulses     = 0;
+volatile int rightPrevPulses = 0;
+volatile float rightAngle =0;
+volatile float rightPwmDuty  = 0.0;
+volatile float rightPwmDutyPrev = 0.0;
+volatile float rightVelocity = 0.0;
+float rightRequest=0;
+
+float request;
+float request_pos;
+float request_neg;
+float round=27720;
+float maxspeed=10; //cm/sec
+float currentX;
+float currentY;
+float l=10;
+float toX;
+float toY;
+float toLeftAngle;
+float toRightAngle;
+float leftDeltaAngle;
+float rightDeltaAngle;
+float M_PI=3.14159265359;
+
+
+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(-180.0, 180.0);
+    leftController.setOutputLimits(-1.0, 1.0);
+    leftController.setBias(0);
+    leftController.setDeadzone(-0.4, 0.4);
+    leftController.setMode(AUTO_MODE);
+    
+    rightController.setInputLimits(-180.0, 180.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