Diff: main.cpp

Revision:

8:7cc4d6d9c2b5

Parent:

7:742b1969f6c9

Child:

9:6a065971d0ae

--- a/main.cpp	Thu Oct 20 12:06:03 2016 +0000
+++ b/main.cpp	Thu Oct 20 13:33:47 2016 +0000
@@ -12,7 +12,7 @@
 Serial pc(USBTX,USBRX);
 QEI encoder(D12,D13,NC,32);
 HIDScope scope(1);
-
+DigitalOut led(D0);
 //*******************Setting tickers and printers*******************
 Ticker callMotor;
 Ticker angPos;
@@ -32,12 +32,12 @@
 const double pi = 3.14159265359;
 const double ts = 1.0/1000.0;
 const int velocityChannel = 0;
-volatile double radians =0.0;
 const double transmissionShoulder =94.4/40.2;
 const double transmissionElbow = 1.0;
 const double MOTOR1_KP = 2.5;
 const double CONTROLLER_TS = 0.01;
-volatile double proportionalErrormotor1 = -1;
+volatile double radians;
+volatile double proportionalErrormotor1;
 //*****************Angles Arms***********************
 
 double O1=1.7633;
@@ -54,28 +54,29 @@
 double B6=-0.8994;
 
 //**********functions******************************
-double getAngPosition() 
+void getAngPosition() 
 {   
     volatile int pulses = encoder.getPulses();
     radians = (pulses / (2 * 3591.84)) * 2*pi; //2 = encoding type, 3591.84 = counts per revoluton for the two big motors
-return radians;
 }
   
  // Next task, measure the error and apply the output to the plant
- void motor1_Controller() 
+ void motor1_Controller(double radians)
  {
  double reference = -2*pi;
- double position = radians;
- double error1 = reference - position;
- double proportionalErrormotor1 = error1 * MOTOR1_KP;
-     scope.set(velocityChannel,proportionalErrormotor1);
-     scope.send();
+ volatile double error1 = reference - radians;
+ proportionalErrormotor1 = error1 * MOTOR1_KP;
+  // scope.set(0, proportionalErrormotor1);
+   //scope.send();
+   
+   // scope.set(velocityChannel,proportionalErrormotor1);
+    //scope.send();
  }
   
   
 void control(double proportionalErrormotor1)
 {
-if(proportionalErrormotor1>0)
+if(proportionalErrormotor1<0)
         {
         motor1MagnitudePin=1.0;//MOET NOG TUSSENWAAREN KRIJGEN
         }
@@ -85,23 +86,24 @@
         }
 }
 
+
  
 //****************MAIN FUNCTION*********************************
 int main()
-{
-t1.attach(&fn1_activate, 0.0001f);
+{motor1MagnitudePin.period(1.0/1000.0);
+t1.attach(&fn1_activate, 0.1f);
 t2.attach(&fn2_activate, 0.0001f);
 t3.attach(&fn3_activate, 0.0001f);
 while(true){
         if(fn1_go)
         {
         fn1_go = false;
-        control(radians);
+        control(proportionalErrormotor1);
         }
         if(fn2_go)
         {
         fn2_go = false;
-        motor1_Controller();
+        motor1_Controller(radians);
         }
         if(fn3_go)
         {
@@ -109,5 +111,5 @@
         getAngPosition();
         }
 }
-    pc.baud(115200);
+
 }
\ No newline at end of file