Diff: main.cpp

Revision:

3:13bd725bd47b

Parent:

2:ca6d8d1f77d4

Child:

4:49b5cde0aecc

--- a/main.cpp	Thu Aug 18 14:29:47 2016 +0000
+++ b/main.cpp	Sat Aug 20 18:24:21 2016 +0000
@@ -25,125 +25,17 @@
     float pos, avgtemp, joyval, maxval;
     unsigned char chk;
     bool cont, quit;
+    //My adds
     LowPowerTimer time;
+    SSC_32 ser;
     //log.init("logfile", true);
     time.start();
     inittime = time.read();
-/*
-    if (SDL_Init(SDL_INIT_VIDEO | SDL_INIT_JOYSTICK) < 0)
-    {
-        Ordi->log( "ERROR in SDL init: " << SDL_GetError() );
-        return -1;
-    }
-    atexit(SDL_Quit);
-    screen = SDL_SetVideoMode(800, 480, 32, SDL_NOFRAME | SDL_FULLSCREEN);
-    SDL_ShowCursor(0);
-    SDL_JoystickEventState(SDL_ENABLE);
-    // sort out joystick control
-    if (SDL_NumJoysticks()>0)
-    {
-        joy=SDL_JoystickOpen(0);
-        if (joy)
-        {
-            Ordi->log( "Joystick Connected." );
-        } else {
-            Ordi->log( "ERROR: could not connect to joystick!" );
-            return -1;
-        }
-    } else {
-        Ordi->log("ERROR: could not find joystick!" );
-        return -1;
-    }
-*/
-    // UDOO to Due is /dev/ttymxc3
-    ser.init_old("/dev/ttymxc3", false);
-    
-    Ordi->log("Press Start to connect." );
-    cont = false;
-    while (!cont)
-    {
-        while (SDL_PollEvent(&event))
-        {
-            switch (event.type)
-            {
-                case SDL_JOYBUTTONDOWN:
-                    if (event.jbutton.button == 7) cont = true;
-                    break;
-            }
-        }
-        SDL_Delay(10);
-    }
-
-    // prep the motor enable / disable packets
-    pack_enable = new packet(18, 'A');
-    pack_enable->data[0] = 0x03;
-    pack_disable = new packet(18, 'A');
-    pack_disable->data[0] = 0x03;
-    for (ii=0; ii<18; ii++)
-    {
-        pack_enable->data[ii+1] = 0x01;
-        pack_disable->data[ii+1] = 0x00;
-    }
-    
-    // before continuing, ask scontroller for servo data
-    // mostly to make sure it's ready to do stuff
-    Ordi->log("Confirming Connection.." );
-    // step 1: create a packet destined for the arbotix-m
-    pack = new packet(16, 'A'); // reasonable size?
-    // step 2: set command byte to 0x05, request for data
-    pack->data[0] = 0x05;
-    // step 3: set request byte to 0x01, requesting servo angles
-    pack->data[1] = 0x01;
-    // step 4: set return byte to 'D', send back to Due
-    pack->data[2] = 'U';
-    // step 5: send the packet
-    ser.send(pack, true);
-    sleep(1);
-    
-    // the Arbotix-M might be off or initializing, so keep sending the request
-    // until you hear something back
-    pack2 = NULL;
-    while ((pack2 = ser.recv('U', false)) == NULL)
-    {
-        ser.send(pack, true);
-        sleep(1);
-    }
-    delete pack;
-    
-    // we received data from the Arbotix-M, it's good to continue
-    Ordi->log( "System Ready." );
-    for (ii=0; ii<18; ii++)
-    {
-        memcpy(&pos, pack2->data+1+ii*(sizeof(float)+sizeof(uint8_t)), 
-                sizeof(float));
-        Ordi->log( pos );
-        hex.servoangle[ii] = pos;
-    }
-    hex.setAngles();
-    delete pack2;
-
-//rien qui ne nous interesse jusque ici
-    Ordi->log( "Angles read." );
-    
-    // sit / stand
-    dsize = 100;
-    psize = SIZE;
-    pack = new packet(dsize, 'A', psize);
-    pack->data[0] = 0x01; // set servo positions
 
     // max useable speed is 2.0 -> 1 foot per second
     hex.speed = 0.0; // in cycles per second
     hex.turning = 0.0; // [-1,1], rotation in z-axis
 
-    //ser.send(pack_enable);
-
-    // get ready to ask for data
-    pack_ask = new packet(16, 'A');
-    pack_ask->data[0] = 0x05;
-    pack_ask->data[1] = 0x02; // want temperature
-    pack_ask->data[2] = 'U';
-    pack_data = NULL;
-    
     Ordi->log( "Begin IK" );
     // IK test
     time = 0.0;
@@ -151,78 +43,40 @@
     hex.safeStand();
     while (hex.ssrunning)
     {
-        dt = (getTime() - lasttime);
-        lasttime = getTime();
+        dt = (time.read() - lasttime);
+        lasttime = time.read();
         hex.step(dt);
         // package positions
         for (ii=0; ii<18; ii++)
         {
-            pos = hex.servoangle[ii];
-            memcpy(pack->data+1+ii*sizeof(float), 
-                    &pos, sizeof(float));
+            ser.add(ii,hex.servoangle[ii]);
+            
         }
-        ser.send(pack);
-        usleep(20*1000);
+        ser.send();
+        wait_us(20*1000);
 
     }
-    lasttime = getTime();
+    lasttime = time.read();
     lastdata = lasttime;
-    ser.send(pack_ask);
-    usleep(20*1000);
+    ser.send();
+    wait_us(20*1000);
 
     quit = false;
     while (!quit)
     {
-        dt = (getTime() - lasttime);
+        dt = (time.read() - lasttime);
         time += dt;
-        lasttime = getTime();
+        lasttime = time.read();
         hex.step(dt);
-        d = new data_chunk('P', inittime);
-        d->add(hex.dr_ang);
-        d->add(hex.dr_xpos);
-        d->add(hex.dr_ypos);
-        d->add(hex.maxsweep);
-        d->add(hex.speedmodifier);
-        log.send(d);
         // package positions
         for (ii=0; ii<18; ii++)
         {
-            pos = hex.servoangle[ii];
-            memcpy(pack->data+1+(ii)*sizeof(float), 
-                    &pos, sizeof(float));
+            ser.add(ii, hex.servoangle[ii]);
         }
-        ser.send(pack);
-        // ask for data?
+        ser.send();
+        // ask for data?        
         
-        if (getTime() - lastdata > 1.0)
-        {
-        if ((pack2=ser.recv('U',false)) != NULL)
-        {
-            maxval = 1e-10;
-            avgtemp = 0.0;
-            for (ii=0; ii<18; ii++)
-            {
-                memcpy(&pos, pack2->data+1+ii*(sizeof(float)+sizeof(uint8_t)), 
-                        sizeof(float));
-                memcpy(&errcode, pack2->data+1+ii*(sizeof(float)+sizeof(uint8_t))+sizeof(float), sizeof(uint8_t));
-                avgtemp += pos;
-                if (pos > maxval) maxval = pos;
-                if (errcode != 0) // 32=overload, 4=temperature, 1=voltage
-                    Ordi->log( "SERVO ERROR: "<< (int)(errcode) << " ON SERVO " << ii );
-            }
-            avgtemp /= 18.;
-            d = new data_chunk('T', inittime);
-            d->add(avgtemp);
-            log.send(d);
-            delete pack2;
-            lastdata = getTime();
-            pack2 = NULL;
-            ser.send(pack_ask);
-            usleep(20*1000);
-        }
-        }
-        
-        while (SDL_PollEvent(&event))
+        while (Ordi->buffer_has_recived()!=false)
         {
             switch (event.type)
             {
@@ -257,8 +111,6 @@
 
     Ordi->log( "Quitting.." );
     ser.send(pack_disable);
-    delete pack;
-    log.close();
     ser.close();
     return 0;
     }