Diff: main.cpp

Revision:

4:778bc352c47f

Parent:

3:10fa3102c2d7

Child:

5:01e1e68309ae

--- a/main.cpp	Fri Oct 05 15:57:55 2018 +0000
+++ b/main.cpp	Sun Oct 07 19:40:12 2018 +0000
@@ -3,6 +3,9 @@
 #include "lwip-interface/EthernetInterface.h"
 #include "comms.h"
 #include <string>
+#include "calibration.h"
+#include "Axis.h"
+#include "kinematics.h"
 #include "BufferedSerial.h"
 DigitalOut led1(LED1);
 DigitalOut homeGND(PF_5);
@@ -10,13 +13,16 @@
 DigitalIn homeSwitchB(PC_0);
 DigitalIn homeSwitchC(PC_3);
 DigitalIn trigger(PF_3);
+BufferedSerial buffered_pc(SERIAL_TX, SERIAL_RX);
+calVals calibration;
+
 
 using std::string;
 const int BROADCAST_PORT_T = 58080;
 const int BROADCAST_PORT_R = 58081;
 EthernetInterface eth;
 
-BufferedSerial buffered_pc(SERIAL_TX, SERIAL_RX);
+
 
 
 void transmit()
@@ -68,71 +74,40 @@
     ODSerial1.baud(115200);
     ODrive OD0(ODSerial0);
     ODrive OD1(ODSerial1);
-    int requested_state;
-    requested_state = ODrive::AXIS_STATE_CLOSED_LOOP_CONTROL;
-    OD0.run_state(0, requested_state, false); // don't wait
-    OD1.run_state(0, requested_state, false); // don't wait
-    OD1.run_state(1, requested_state, false); // don't wait
-    Thread::wait(1000);
-//   OD0.SetVelocity(0,-1000);
-//    OD1.SetVelocity(1,1000);
-    //OD1.SetVelocity(1,1000);
-//    OD1.SetVelocity(0,1000);
-//    OD1.SetVelocity(1,1000);
-    
-    bool Ahomed = false;
-    int homeCountA = 0;
-    int homeOffsetA = 0;
-    while(!Ahomed) {
-        OD1.SetPosition(1,homeOffsetA);
-        Thread::wait(1);
-        for(int i = 0 ; i < 99; i++) {
-            if(homeSwitchA == false) {
-                homeCountA++;
-            } else {
-                homeCountA = 0;
-            }
-            if(homeCountA > 100) {
-                Ahomed = true;
-            }
-        }
-        homeCountA= 0;
-        homeOffsetA++;
-    }
-    
-    
-        bool Bhomed = false;
-    int homeCountB = 0;
-    int homeOffsetB = 0;
-    while(!Bhomed) {
-        OD1.SetPosition(0,homeOffsetB);
-        Thread::wait(1);
-        for(int i = 0 ; i < 99; i++) {
-            if(homeSwitchB == false) {
-                homeCountB++;
-            } else {
-                homeCountB = 0;
-            }
-            if(homeCountB > 100) {
-                Bhomed = true;
-            }
-        }
-        homeCountB= 0;
-        homeOffsetB++;
-    }
-
+    //Axis A(&OD1, 1, &homeSwitchA,calibration,AX_A);
+//    Axis B(&OD1, 0, &homeSwitchB,calibration,AX_B);
+//    Axis C(&OD0, 0, &homeSwitchC,calibration,AX_C);
+//    Kinematics kin(&A, &B, &C, calibration); // the Kinematics class contains everything
+//    kin.activateMotors();
+//    kin.homeMotors();
+//    //int error = kin.goToPos(0,0,-10);
+//    kin.goToAngles(pi/4,pi/4,pi/4);
+//    Thread::wait(1000);
+//    float inc = pi /500;
+//    float i = 0;
+//    float k = -100;
+//    int up = 0;
     while(true) {
-        OD0.SetPosition(0,1200);
-        OD1.SetPosition(1,1200);
-        OD1.SetPosition(0,1200);
-        Thread::wait(1000);
-        OD0.SetPosition(0,-2000);
-        OD1.SetPosition(1,-2000);
-        OD1.SetPosition(0,-2000);
-        Thread::wait(1000);
-        string busVrequest = "r vbus_voltage\n";
-        ODSerial0.write(busVrequest.c_str(),busVrequest.length());
-        buffered_pc.printf("bus voltage %f\n",OD0.readFloat());
+ //       int count = 0;
+//        for(int j = 0 ; j < 100; j++) {
+//
+//            if(trigger == false) {
+//                count++;
+//            }
+//        }
+//        if(count>90) {
+//            i += inc;
+//            if(up == 1)k+=0.05;
+//            else k-=0.05;
+//            if(k >= -80) up = 0;
+//            if(k <= -180) up = 1;
+//            //int error = kin.goToPos(40.0*sin(i),40.0*cos(i),k);
+//            //int error = kin.goToPos(0,0,k);
+//            if(i > 2*pi) i = 0;
+            Thread::wait(10);
+            buffered_pc.printf("bus voltage %f\r\n",OD1.readBattery());
+            //buffered_pc.printf("k= %f\r\n",k);
+        //}
     }
 
 }
@@ -140,12 +115,22 @@
 
 int main()
 {
+
+    calibration.e = 58.095;
+    calibration.f = 60.722;
+    calibration.re = 150;
+    calibration.rf = 143.0;
+    calibration.Aoffset = 0.43 - 0.111701;
+    calibration.Boffset = 0.43 - 0.111701;
+    calibration.Coffset = 0.43 - 0.111701;
+    calibration.gearRatio = 89.0/24.0;
     buffered_pc.baud(115200);
     buffered_pc.printf("hello\r\n");
     Thread transmitter;
     Thread receiver;
     Thread odriveThread;
 
+
     //set switches up
     homeGND = false;
     homeSwitchA.mode(PullUp);
@@ -162,7 +147,7 @@
     odriveThread.start(runOdrive);
 
     while (true) {
-        led1 = !led1;
+        //led1 = !led1;
         Thread::wait(500);
     }
 }