Subversion Repositories shark

Rev

Rev 1645 | Rev 1647 | Go to most recent revision | Blame | Compare with Previous | Last modification | View Log | RSS feed

/*
 * Project: S.Ha.R.K.
 *
 * Coordinators: Giorgio Buttazzo <giorgio@sssup.it>
 *
 * ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy)
 *
 * http://www.sssup.it
 * http://retis.sssup.it
 * http://shark.sssup.it
 */


#include "chimera.h"

#define POS_B_REF        0
#define POS_B_UP        50

int vel_dx = 15;
int vel_sx = 15;

void program_key_end(KEY_EVT* e)
{

        int i;
        TIME tmp;

        end_send();

        for (i=3; i<MAX_PROC; i++){
                if (!jet_getstat(i, NULL, &tmp, NULL, NULL))
                        kern_printf("Task Name : %s - Max Time  : %d\n", proc_table[i].name, (int)tmp);
        }

        sys_end();

}

int beta = 30;

void action_stand_up(void) {
                                                                                                                             
  struct action_event e;
  int event,alfa;
 
  status.power = 1;

  kern_gettime(&(e.time));

  for (alfa=0;alfa<=90;alfa+=15) {

  ADDUSEC2TIMESPEC(5000000,&(e.time));
                                                                                                                             
  e.type = EVT_SET_MASK_LEG_ANGLE;
  e.mask = 0x3F;
  e.ang.a = alfa * 3600;
  e.ang.b = beta * 3600;
  e.ang.c = 0 * 3600;
  e.pwm = 7;
                                                                                                                             
  event = insert_action_event(&(e));
  cprintf("Event num = %d\n",event);

  }

}

TASK walk_auto_6()
{
        short   i, n = 0;
        int     vd, vs, auto_count = 0;
        int pos_a[6], pos_b[6], pos_c[6];
        struct action_event e;

        float   fake_sin[36] = { .00, .50, .80, 1.0, .80, .50, .00, .00, .00, .00, .00, .00,
                                 .00, .00, .00, .00, .00, .00, .00, .00, .00, .00, .00, .00,
                                 .00, .00, .00, .00, .00, .00, .00, .00, .00, .00, .00, .00};

        float   fake_cos[36] = {-1.0,-.75,-.50, .00, .50, .75, 1.0, .93, .86, .80, .73, .66,
                                 .60, .53, .46, .40, .33, .26, .20, .13, .06, .00,-.06, .13,
                                -.20,-.26,-.33,-.40,-.46,-.53,-.60,-.66,-.73,-.80,-.86,-.93};

        while (1) {
                vd = vel_dx;
                vs = vel_sx;

                kern_gettime(&(e.time));
                ADDUSEC2TIMESPEC(10000,&(e.time));
                           
                for (i = 0; i<6; i++) {
                        switch(i) {
                                case 0:
                                        n = 0;
                                        break;
                                case 1:
                                        n = 3;
                                        break;
                                                                                                                             
                                case 2:
                                        n = 4;
                                        break;
                                                                                                                             
                                case 3:
                                        n = 1;
                                        break;
                                                                                                                             
                                case 4:
                                        n = 2;
                                        break;
                                                                                                                             
                                case 5:
                                        n = 5;
                                        break;
                        }                                                                                                                            
                        if (i%3)
                                pos_c[i] = vd * fake_cos[(auto_count+6*n)%36];
                        else
                                pos_c[i] = vs * fake_cos[(auto_count+6*n)%36];
                        if ((vs) || (vd))
                                pos_b[i] = POS_B_REF + fake_sin[(auto_count+6*n)%36] * POS_B_UP;
                        else
                                pos_b[i] = POS_B_REF;

                        e.type = EVT_SET_MASK_LEG_ANGLE;
                        e.mask = 1 << i;
                        e.ang.a = (80) * 3600;
                        e.ang.b = (pos_b[i]) * 3600;
                        e.ang.c = (pos_c[i]) * 3600;
                        e.pwm = 7;
                        insert_action_event(&(e));
                }
                auto_count++;
                if (auto_count > 35) auto_count = 0;

                task_testcancel();
                task_endcycle();
        }
        return 0;
}

TASK walk_auto_3()
{
        short   i;
        int     vd, vs, auto_count = 0;
        float   cos_a;
        int pos_a[6], pos_b[6], pos_c[6];

        struct action_event e;


        float   fake_cos[32] = { 1.0000, .99518, .98079, .95694, .92388, .88192, .83147, .77301,
                                 .70711, .63439, .55557, .47140, .38268, .29028, .19509, .09802,
                                 .00000,-.09802,-.19509,-.29028,-.38268,-.47140,-.55557,-.63439,
                                -.70711,-.77301,-.83147,-.88192,-.92388,-.95694,-.98079,-.99518};
        float   fake_sin[64] = { -0.50, -0.45, -0.40, -0.30, -0.20, -0.15, -0.10, -0.05,
                                  0.00,  0.00,  0.40,  0.40,  0.70,  0.70,  0.90,  0.90,
                                  1.00,  1.00,  0.90,  0.90,  0.70,  0.70,  0.40,  0.40,
                                  0.00,  0.00, -0.05, -0.10, -0.15, -0.20, -0.30, -0.40,
                                 -0.45, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50,
                                 -0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50,
                                 -0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50,
                                 -0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50 };

        while (1) {
                cos_a = (auto_count < 32) ? fake_cos[auto_count] : -fake_cos[auto_count-32];

                vd = vel_dx;
                vs = vel_sx;

                kern_gettime(&(e.time));
                ADDUSEC2TIMESPEC(10000,&(e.time));
                           
                for (i = 0; i<6; i++) {
                        switch (i) {
                                case 0:
                                        /* Leg 0 */
                                        pos_c[i] =  vs * cos_a;
                                        if ((vs) || (vd))
                                                pos_b[i] = POS_B_REF + fake_sin[(auto_count+32)%64] * POS_B_UP;
                                        else
                                                pos_b[i] = POS_B_REF;
                                        break;
                                case 1:
                                        /* Leg 1 */
                                        pos_c[i] = -vd * cos_a;
                                        if ((vs) || (vd))
                                                pos_b[i] = POS_B_REF + fake_sin[auto_count] * POS_B_UP;
                                        else
                                                pos_b[i] = POS_B_REF;
                                        break;
                                case 2:
                                        /* Leg 2 */
                                        pos_c[i] = -vs * cos_a;
                                        if ((vs) || (vd))
                                                pos_b[i] = POS_B_REF + fake_sin[auto_count] * POS_B_UP;
                                        else
                                                pos_b[i] = POS_B_REF;
                                        break;
                                case 3:
                                        /* Leg 3 */
                                        pos_c[i] =  vd * cos_a;
                                        if ((vs) || (vd))
                                                pos_b[i] = POS_B_REF + fake_sin[(auto_count+32)%64] * POS_B_UP;
                                        else
                                                pos_b[i] = POS_B_REF;
                                        break;
                                case 4:
                                        /* Leg 4 */
                                        pos_c[i] =  vs * cos_a;
                                        if ((vs) || (vd))
                                                pos_b[i] = POS_B_REF + fake_sin[(auto_count+32)%64] * POS_B_UP;
                                        else
                                                pos_b[i] = POS_B_REF;
                                        break;
                                case 5:
                                        /* Leg 5 */
                                        pos_c[i] = -vd * cos_a;
                                        if ((vs) || (vd))
                                                pos_b[i] = POS_B_REF + fake_sin[auto_count] * POS_B_UP;
                                        else
                                                pos_b[i] = POS_B_REF;
                                        break;
                        }

                        e.type = EVT_SET_MASK_LEG_ANGLE;
                        e.mask = 1 << i;
                        e.ang.a = (80) * 3600;
                        e.ang.b = (pos_b[i]) * 3600;
                        e.ang.c = (pos_c[i]) * 3600;
                        e.pwm = 7;
                        insert_action_event(&(e));
                }

                auto_count += 4;
                if (auto_count > 63) auto_count = 0;

                task_testcancel();
                task_endcycle();
        }
        return 0;
}

void action_walk(void) {
        HARD_TASK_MODEL ms;
        PID pid_walk;
                                                                                                                             
        hard_task_default_model(ms);
        hard_task_def_ctrl_jet(ms);
        hard_task_def_wcet(ms, 1000);
        hard_task_def_mit(ms, 60000);
        hard_task_def_usemath(ms);
        pid_walk = task_create("Walk_Task", walk_auto_3, &ms, NULL);
        if (pid_walk == NIL) {
                perror("Could not create task <Walk_Auto>");
                sys_end();
        } else
                task_activate(pid_walk);
}

int main(int argc, char **argv)
{
        TIME seme;
        KEY_EVT k;

        seme = sys_gettime(NULL);
        srand(seme);

        k.flag = ALTL_BIT;
        k.scan = KEY_C;
        k.ascii = 'c';
        keyb_hook(k,program_key_end);

        clear();

        init_send();            /* Comunication */
        init_key();             /* Keyboard */

        init_action_event(100);

        action_walk();
        //action_stand_up();

        return 0;

}