nxtOSEK/samples/nxtgt/nxtgt.c

   1 /* nxtgt.c */ \r
   2 #include "kernel.h"\r
   3 #include "kernel_id.h"\r
   4 #include "ecrobot_interface.h"\r
   5 \r
   6 /* OSEK declarations */\r
   7 DeclareCounter(SysTimerCnt);\r
   8 DeclareTask(TaskInitialize);\r
   9 DeclareTask(TaskControl);\r
  10 DeclareTask(TaskSonar);\r
  11 DeclareTask(TaskLCD);\r
  12 \r
  13 /* Definitions */\r
  14 #define MOTOR_STEERING     NXT_PORT_A\r
  15 #define MOTOR_LEFT         NXT_PORT_B\r
  16 #define MOTOR_RIGHT        NXT_PORT_C\r
  17 #define STEERING_LIMIT             40 /* degree */\r
  18 #define STEERING_P_GAIN             2 /* proportinal gain */\r
  19 #define DIFF_GAIN_MAX            0.7F /* 1.0-DIFF_GAIN_MAX: max. differential effect */\r
  20 #define NEUTRAL_DEAD_ZONE           2 /* degree */\r
  21 #define PWM_OFFSET                 10 /* friction compensator */\r
  22 #define EDC_ON                     -1 /* Electronic Differential Control: ON */\r
  23 #define EDC_OFF                     1 /* Electronic Differential Control: OFF */\r
  24 \r
  25 static S8 EDC_flag;                                /* EDC flag */\r
  26 \r
  27 /* Prototypes */\r
  28 S32 FrictionComp(S32 ratio, S32 offset);\r
  29 \r
  30 /* LEJOS OSEK hooks */\r
  31 void ecrobot_device_initialize()\r
  32 {\r
  33         ecrobot_set_light_sensor_active(NXT_PORT_S1);\r
  34         ecrobot_set_light_sensor_active(NXT_PORT_S3);\r
  35         ecrobot_init_sonar_sensor(NXT_PORT_S2);\r
  36         ecrobot_init_bt_connection();\r
  37 }\r
  38 \r
  39 void ecrobot_device_terminate()\r
  40 {\r
  41         nxt_motor_set_speed(MOTOR_STEERING, 0, 1);\r
  42         nxt_motor_set_speed(MOTOR_RIGHT, 0, 1);\r
  43         nxt_motor_set_speed(MOTOR_LEFT, 0, 1);\r
  44         ecrobot_set_light_sensor_inactive(NXT_PORT_S1);\r
  45         ecrobot_set_light_sensor_inactive(NXT_PORT_S3);\r
  46         ecrobot_term_sonar_sensor(NXT_PORT_S2);\r
  47         ecrobot_term_bt_connection();\r
  48 }\r
  49 \r
  50 void user_1ms_isr_type2(void)\r
  51 {\r
  52         StatusType ercd;\r
  53 \r
  54         ercd = SignalCounter(SysTimerCnt); /* Increment OSEK Alarm Counter */\r
  55         if(ercd != E_OK)\r
  56         {\r
  57         ShutdownOS(ercd);\r
  58         }\r
  59 }\r
  60 \r
  61 /* TaskInitialize */\r
  62 TASK(TaskInitialize)\r
  63 {\r
  64         nxt_motor_set_speed(MOTOR_STEERING, 0, 1);\r
  65         nxt_motor_set_speed(MOTOR_RIGHT, 0, 1);\r
  66         nxt_motor_set_speed(MOTOR_LEFT, 0, 1);\r
  67         nxt_motor_set_count(MOTOR_STEERING, 0);\r
  68         nxt_motor_set_count(MOTOR_RIGHT, 0);\r
  69         nxt_motor_set_count(MOTOR_LEFT, 0);\r
  70 \r
  71         EDC_flag = EDC_OFF; \r
  72 \r
  73         TerminateTask();\r
  74 }\r
  75 \r
  76 /* TaskControl executed every 10msec */\r
  77 TASK(TaskControl)\r
  78 {\r
  79         S32 analog_stick_left;  /* speed command data from GamePad */\r
  80         S32 analog_stick_right; /* steering command data from GamePad */\r
  81         S32 steering_angle;\r
  82         S32 steering_err;\r
  83         S32 steering_speed;\r
  84         S32 diff_gain;\r
  85         U8 touch_sensor;\r
  86         static U8 touch_sensor_state = 0;\r
  87         static U8 bt_receive_buf[32];  /* buffer size is fixed as 32 */ \r
  88         \r
  89         /* receive NXTGamePad command\r
  90      * byte0 speed_data    -100(forward max.) to 100(backward max.)\r
  91      * byte1 steering_data -100(left max.) to 100(right max.)\r
  92      */\r
  93         ecrobot_read_bt_packet(bt_receive_buf, 32);\r
  94         analog_stick_left = -(S32)(*(S8 *)(&bt_receive_buf[0])); /* reverse the direction */\r
  95         analog_stick_right = (S32)(*(S8 *)(&bt_receive_buf[1]));\r
  96 \r
  97         /* read Touch Sensor to switch Electronic Differential Control */\r
  98         touch_sensor = ecrobot_get_touch_sensor(NXT_PORT_S4);\r
  99         if (touch_sensor == 1 && touch_sensor_state == 0)\r
 100         {\r
 101         EDC_flag = ~EDC_flag + 1; /* toggle */\r
 102         }\r
 103         touch_sensor_state = touch_sensor;\r
 104 \r
 105         /* steering control */\r
 106         steering_angle = nxt_motor_get_count(MOTOR_STEERING);\r
 107         steering_err = (STEERING_LIMIT*analog_stick_right)/100 - steering_angle;\r
 108         steering_speed = STEERING_P_GAIN*steering_err;\r
 109         nxt_motor_set_speed(MOTOR_STEERING, FrictionComp(steering_speed,PWM_OFFSET), 1);\r
 110 \r
 111         /* wheel motors control with Electronic Differential Control */\r
 112         diff_gain = 10; \r
 113         if (steering_angle > NEUTRAL_DEAD_ZONE) /* turn right */\r
 114         {\r
 115         if (EDC_flag == EDC_ON)\r
 116         {\r
 117                 diff_gain = (S32)((1.0F - (float)steering_angle*DIFF_GAIN_MAX/STEERING_LIMIT)*10);\r
 118         }\r
 119         nxt_motor_set_speed(MOTOR_RIGHT, FrictionComp((analog_stick_left*diff_gain)/10,PWM_OFFSET), 1);\r
 120         nxt_motor_set_speed(MOTOR_LEFT, FrictionComp(analog_stick_left,PWM_OFFSET), 1);\r
 121         }\r
 122         else if (steering_angle < -NEUTRAL_DEAD_ZONE) /* turn left */\r
 123         {\r
 124         if (EDC_flag == EDC_ON)\r
 125         {\r
 126                 diff_gain = (S32)((1.0F + (float)steering_angle*DIFF_GAIN_MAX/STEERING_LIMIT)*10);\r
 127         }\r
 128         nxt_motor_set_speed(MOTOR_RIGHT, FrictionComp(analog_stick_left,PWM_OFFSET), 1);\r
 129         nxt_motor_set_speed(MOTOR_LEFT, FrictionComp((analog_stick_left*diff_gain)/10,PWM_OFFSET), 1);\r
 130         }\r
 131         else /* go straight */\r
 132         {\r
 133         nxt_motor_set_speed(MOTOR_RIGHT, FrictionComp(analog_stick_left,PWM_OFFSET), 1);\r
 134         nxt_motor_set_speed(MOTOR_LEFT, FrictionComp(analog_stick_left,PWM_OFFSET), 1);\r
 135         }\r
 136 \r
 137         /* send NXT status data to NXT GamePad */\r
 138         ecrobot_bt_data_logger((S8)analog_stick_left, (S8)analog_stick_right);\r
 139 \r
 140         TerminateTask();\r
 141 }\r
 142 \r
 143 /* TaskSonar executed every 50msec */\r
 144 TASK(TaskSonar)\r
 145 {\r
 146         S32 sonar;\r
 147 \r
 148         /* Sonar Sensor is invoked just for data logging */\r
 149         sonar = ecrobot_get_sonar_sensor(NXT_PORT_S2);\r
 150 \r
 151         TerminateTask();\r
 152 }\r
 153 \r
 154 /* TaskLCD executed every 500msec */\r
 155 TASK(TaskLCD)\r
 156 {\r
 157         ecrobot_status_monitor("NXT GT");\r
 158 \r
 159         TerminateTask();\r
 160 }\r
 161 \r
 162 /* Sub functions */\r
 163 S32 FrictionComp(S32 ratio, S32 offset)\r
 164 {\r
 165         if (ratio > 0)\r
 166         {\r
 167         return ((100-offset)*ratio/100 + offset);\r
 168         }\r
 169         else if (ratio < 0)\r
 170         {\r
 171         return ((100-offset)*ratio/100 - offset);\r
 172         }\r
 173         else\r
 174         {\r
 175         return ratio;\r
 176         }\r
 177 }\r