CPAL Playground

/* Copyright (C) 2015, RealTime-at-Work, All Rights Reserved
 *
 * You may use, copy, reproduce, and distribute this sample cource code for any non-commercial purpose.
 * Some purposes which can be non-commercial are teaching, academic research,
 * public demonstrations and personal experimentation. You may also distribute this Software
 * with books or other teaching materials, or publish the Software on websites,
 * that are intended to teaching for academic or other non-commercial purposes.
 *
 * In return, we simply require that you agree: 
 * 1. That you will not remove any copyright or other notices from the Software.
 * 2. THAT THE SOFTWARE COMES "AS IS", WITH NO WARRANTIES. THIS MEANS NO EXPRESS, 
 *    IMPLIED OR STATUTORY WARRANTY.
 *
 * @keywords IMU; frdm k64f
 *
 * See FXOS8700CQ.pdf for 6-axis sensor available on the board.
 * The IMU is configured in hybrid mode, 4g mode.
 *
 * This program prints all IMU values on the serial links and changes color of the LED depending on inclination.
 *
 * Initially it considers the board to be on a flat surface to compute offsets.
 * Indeed the board is always subject to gravity (value -2048 on the Z axis).
 * 
 * The LED is red blinking when the board has no particular orientation
 * Admitting the board is in landscape mode with FRDM label on right and USB on left, color is as follow:
 * 
 *    --------------------------------
 *    |                 White        |
 *    sda usb                        |
 *    |                              |
 *    Ethernet   Green            Red|
 *    |                              |
 *    k64 usb                        |
 *    |                 Blue         |
 *    --------------------------------
 *
 *
 * Reminder: uses following command to see serial port: minicom -D /dev/ttyACM0
 *
 * NOTE: there is an important drift and thus the program is not very stable in time.
 */

set_red(out bool: r, out bool: g, out bool: b)
{
    r = true;
    g = false;
    b = false;
}

set_green(out bool: r, out bool: g, out bool: b)
{
    r = false;
    g = true;
    b = false;
}

set_blue(out bool: r, out bool: g, out bool: b)
{
    r = false;
    g = false;
    b = true;
}

set_white(out bool: r, out bool: g, out bool: b)
{
    r = true;
    g = true;
    b = true;
}

set_black(out bool: r, out bool: g, out bool: b)
{
    r = false;
    g = false;
    b = false;
}

struct Imu {
  int16: x;
  int16: y;
  int16: z;
  /* acceleration in multiple of 0.488mg */
  int16: accel_x;
  int16: accel_y;
  int16: accel_z;
};

const int16: MIN_ACCEL = 150;

processdef Inclinaison_Displayer(
  in Imu: imu,
  out bool: red,
  out bool: green,
  out bool: blue)
{
  static var Imu: offsets = {0, 0, 0, 0, 0, 0};
  static var int16: abs_x;
  static var int16: abs_y;

common {
    var uint64: accel_pow_2 = 0;
    imu.accel_x = imu.accel_x + offsets.accel_x;
    imu.accel_y = imu.accel_y + offsets.accel_y;
    imu.accel_z = imu.accel_z + offsets.accel_z;
    accel_pow_2 = accel_pow_2 + uint64.as(int64.as(imu.accel_x) * int64.as(imu.accel_x));
    accel_pow_2 = accel_pow_2 + uint64.as(int64.as(imu.accel_y) * int64.as(imu.accel_y));
    accel_pow_2 = accel_pow_2 + uint64.as(int64.as(imu.accel_z) * int64.as(imu.accel_z));

abs_x = imu.accel_x;
    if (abs_x < 0) {
      abs_x = -abs_x;
    }
    abs_y = imu.accel_y;
    if (abs_y < 0) {
      abs_y = -abs_y;
    }   
    
    /*IO.println("a_x=%i,\ta_y=%i,\ta_z=%i,\tx=%i,\ty=%i,\tz=%i %i %i", imu.accel_x, imu.accel_y, imu.accel_z, imu.x, imu.y, imu.z, abs_x, abs_y);*/
      
 
    
  }

state Init {
    IO.println("Initialising...");
    set_red(red, green, blue);
  }
  on (true) to Offsetting;

state Offsetting {
    /* See AN4069.pdf from freescale*/
    offsets.accel_x = -imu.accel_x;
    offsets.accel_y = -imu.accel_y;
    offsets.accel_z = 2048 - imu.accel_z;
    IO.println("offsets: %i %i %i", offsets.accel_x, offsets.accel_y, offsets.accel_z);
  }
  on (true) to Main;
  
  state Main {
    if (abs_x < MIN_ACCEL and abs_y < MIN_ACCEL) {
      /* red is blinking */
      green = false;
      blue = false;
    } else {
      if (abs_x > abs_y) {
        if (imu.x < -MIN_ACCEL) {
          set_white(red, green, blue);
        } else {
          set_blue(red, green, blue);
        }
      } else {
        if (imu.y < -MIN_ACCEL) {
          set_green(red, green, blue);
        } else {
/*          assert(imu.y > MIN_ACCEL);*/
          set_red(red, green, blue);
        }
      }
    }
  }
  on (abs_x > MIN_ACCEL or abs_y > MIN_ACCEL) {
    IO.println("Resetting timer...");
  } to Main;
  after (300ms) {
    assert(abs_x < MIN_ACCEL and abs_y < MIN_ACCEL);
    red = not red;
  } to Main;
}

var Imu: pin_imu;
var bool: pin_led_red = true;
var bool: pin_led_green = true;
var bool: pin_led_blue = true;

process Inclinaison_Displayer: disp[100ms](pin_imu, pin_led_red, pin_led_green, pin_led_blue);

for ms or try the desktop editor with advanced diagram views

Simulation
Tasks diagram


Quiet mode disabled
[0.000000000000:ASSIGN] Assign cpal_system new value: Scheduling_Policy.FIFO
[0.000000000000:imu_abstraction:STARTING_TASK] 'disp'
[0.000000000000:STATE] process "Inclinaison_Displayer", instance "disp", state "Init"
[0.000000000000:PRINTLN] Initialising...
[0.000000000000:ASSIGN] Assign pin_led_red new value: true
[0.000000000000:ASSIGN] Assign pin_led_green new value: false
[0.000000000000:ASSIGN] Assign pin_led_blue new value: false
[0.000000000000:imu_abstraction:TASK_FINISHED] 'disp'
[0.100000000000:imu_abstraction:STARTING_TASK] 'disp'
[0.100000000000:STATE] process "Inclinaison_Displayer", instance "disp", state "Offsetting"
[0.100000000000:PRINTLN] offsets: 0 0 2048
[0.100000000000:imu_abstraction:TASK_FINISHED] 'disp'
[0.200000000000:imu_abstraction:STARTING_TASK] 'disp'
[0.200000000000:STATE] process "Inclinaison_Displayer", instance "disp", state "Main"
[0.200000000000:ASSIGN] Assign pin_led_green new value: false
[0.200000000000:ASSIGN] Assign pin_led_blue new value: false
[0.200000000000:imu_abstraction:TASK_FINISHED] 'disp'
[0.300000000000:imu_abstraction:STARTING_TASK] 'disp'
[0.300000000000:ASSIGN] Assign pin_led_green new value: false
[0.300000000000:ASSIGN] Assign pin_led_blue new value: false
[0.300000000000:imu_abstraction:TASK_FINISHED] 'disp'
[0.400000000000:imu_abstraction:STARTING_TASK] 'disp'
[0.400000000000:ASSIGN] Assign pin_led_green new value: false
[0.400000000000:ASSIGN] Assign pin_led_blue new value: false
[0.400000000000:imu_abstraction:TASK_FINISHED] 'disp'
[0.500000000000:imu_abstraction:STARTING_TASK] 'disp'
[0.500000000000:STATE] process "Inclinaison_Displayer", instance "disp", transition 2
[0.500000000000:ASSIGN] Assign pin_led_red new value: false
[0.500000000000:STATE] process "Inclinaison_Displayer", instance "disp", state "Main"
[0.500000000000:ASSIGN] Assign pin_led_green new value: false
[0.500000000000:ASSIGN] Assign pin_led_blue new value: false
[0.500000000000:imu_abstraction:TASK_FINISHED] 'disp'
[0.600000000000:imu_abstraction:STARTING_TASK] 'disp'
[0.600000000000:ASSIGN] Assign pin_led_green new value: false
[0.600000000000:ASSIGN] Assign pin_led_blue new value: false
[0.600000000000:imu_abstraction:TASK_FINISHED] 'disp'
[0.700000000000:imu_abstraction:STARTING_TASK] 'disp'
[0.700000000000:ASSIGN] Assign pin_led_green new value: false
[0.700000000000:ASSIGN] Assign pin_led_blue new value: false
[0.700000000000:imu_abstraction:TASK_FINISHED] 'disp'
[0.800000000000:imu_abstraction:STARTING_TASK] 'disp'
[0.800000000000:STATE] process "Inclinaison_Displayer", instance "disp", transition 2
[0.800000000000:ASSIGN] Assign pin_led_red new value: true
[0.800000000000:STATE] process "Inclinaison_Displayer", instance "disp", state "Main"
[0.800000000000:ASSIGN] Assign pin_led_green new value: false
[0.800000000000:ASSIGN] Assign pin_led_blue new value: false
[0.800000000000:imu_abstraction:TASK_FINISHED] 'disp'
[0.900000000000:imu_abstraction:STARTING_TASK] 'disp'
[0.900000000000:ASSIGN] Assign pin_led_green new value: false
[0.900000000000:ASSIGN] Assign pin_led_blue new value: false
[0.900000000000:imu_abstraction:TASK_FINISHED] 'disp'
[1.000000000000:imu_abstraction:STARTING_TASK] 'disp'
[1.000000000000:ASSIGN] Assign pin_led_green new value: false
[1.000000000000:ASSIGN] Assign pin_led_blue new value: false
[1.000000000000:imu_abstraction:TASK_FINISHED] 'disp'
[1.100000000000:imu_abstraction:STARTING_TASK] 'disp'
[1.100000000000:STATE] process "Inclinaison_Displayer", instance "disp", transition 2
[1.100000000000:ASSIGN] Assign pin_led_red new value: false
[1.100000000000:STATE] process "Inclinaison_Displayer", instance "disp", state "Main"
[1.100000000000:ASSIGN] Assign pin_led_green new value: false
[1.100000000000:ASSIGN] Assign pin_led_blue new value: false
[1.100000000000:imu_abstraction:TASK_FINISHED] 'disp'
[1.200000000000:imu_abstraction:STARTING_TASK] 'disp'
[1.200000000000:ASSIGN] Assign pin_led_green new value: false
[1.200000000000:ASSIGN] Assign pin_led_blue new value: false
[1.200000000000:imu_abstraction:TASK_FINISHED] 'disp'
[1.300000000000:imu_abstraction:STARTING_TASK] 'disp'
[1.300000000000:ASSIGN] Assign pin_led_green new value: false
[1.300000000000:ASSIGN] Assign pin_led_blue new value: false
[1.300000000000:imu_abstraction:TASK_FINISHED] 'disp'
[1.400000000000:imu_abstraction:STARTING_TASK] 'disp'
[1.400000000000:STATE] process "Inclinaison_Displayer", instance "disp", transition 2
[1.400000000000:ASSIGN] Assign pin_led_red new value: true
[1.400000000000:STATE] process "Inclinaison_Displayer", instance "disp", state "Main"
[1.400000000000:ASSIGN] Assign pin_led_green new value: false
[1.400000000000:ASSIGN] Assign pin_led_blue new value: false
[1.400000000000:imu_abstraction:TASK_FINISHED] 'disp'
[1.500000000000:imu_abstraction:STARTING_TASK] 'disp'
[1.500000000000:ASSIGN] Assign pin_led_green new value: false
[1.500000000000:ASSIGN] Assign pin_led_blue new value: false
[1.500000000000:imu_abstraction:TASK_FINISHED] 'disp'
[1.600000000000:imu_abstraction:STARTING_TASK] 'disp'
[1.600000000000:ASSIGN] Assign pin_led_green new value: false
[1.600000000000:ASSIGN] Assign pin_led_blue new value: false
[1.600000000000:imu_abstraction:TASK_FINISHED] 'disp'
[1.700000000000:imu_abstraction:STARTING_TASK] 'disp'
[1.700000000000:STATE] process "Inclinaison_Displayer", instance "disp", transition 2
[1.700000000000:ASSIGN] Assign pin_led_red new value: false
[1.700000000000:STATE] process "Inclinaison_Displayer", instance "disp", state "Main"
[1.700000000000:ASSIGN] Assign pin_led_green new value: false
[1.700000000000:ASSIGN] Assign pin_led_blue new value: false
[1.700000000000:imu_abstraction:TASK_FINISHED] 'disp'
[1.800000000000:imu_abstraction:STARTING_TASK] 'disp'
[1.800000000000:ASSIGN] Assign pin_led_green new value: false
[1.800000000000:ASSIGN] Assign pin_led_blue new value: false
[1.800000000000:imu_abstraction:TASK_FINISHED] 'disp'
[1.900000000000:imu_abstraction:STARTING_TASK] 'disp'
[1.900000000000:ASSIGN] Assign pin_led_green new value: false
[1.900000000000:ASSIGN] Assign pin_led_blue new value: false
[1.900000000000:imu_abstraction:TASK_FINISHED] 'disp'
[2.000000000000:imu_abstraction:STARTING_TASK] 'disp'
[2.000000000000:STATE] process "Inclinaison_Displayer", instance "disp", transition 2
[2.000000000000:ASSIGN] Assign pin_led_red new value: true
[2.000000000000:STATE] process "Inclinaison_Displayer", instance "disp", state "Main"
[2.000000000000:ASSIGN] Assign pin_led_green new value: false
[2.000000000000:ASSIGN] Assign pin_led_blue new value: false
[2.000000000000:imu_abstraction:TASK_FINISHED] 'disp'
[2.100000000000:imu_abstraction:STARTING_TASK] 'disp'
[2.100000000000:ASSIGN] Assign pin_led_green new value: false
[2.100000000000:ASSIGN] Assign pin_led_blue new value: false
[2.100000000000:imu_abstraction:TASK_FINISHED] 'disp'
[2.200000000000:imu_abstraction:STARTING_TASK] 'disp'
[2.200000000000:ASSIGN] Assign pin_led_green new value: false
[2.200000000000:ASSIGN] Assign pin_led_blue new value: false
[2.200000000000:imu_abstraction:TASK_FINISHED] 'disp'
[2.300000000000:imu_abstraction:STARTING_TASK] 'disp'
[2.300000000000:STATE] process "Inclinaison_Displayer", instance "disp", transition 2
[2.300000000000:ASSIGN] Assign pin_led_red new value: false
[2.300000000000:STATE] process "Inclinaison_Displayer", instance "disp", state "Main"
[2.300000000000:ASSIGN] Assign pin_led_green new value: false
[2.300000000000:ASSIGN] Assign pin_led_blue new value: false
[2.300000000000:imu_abstraction:TASK_FINISHED] 'disp'
[2.400000000000:imu_abstraction:STARTING_TASK] 'disp'
[2.400000000000:ASSIGN] Assign pin_led_green new value: false
[2.400000000000:ASSIGN] Assign pin_led_blue new value: false
[2.400000000000:imu_abstraction:TASK_FINISHED] 'disp'
[2.500000000000:imu_abstraction:STARTING_TASK] 'disp'
[2.500000000000:ASSIGN] Assign pin_led_green new value: false
[2.500000000000:ASSIGN] Assign pin_led_blue new value: false
[2.500000000000:imu_abstraction:TASK_FINISHED] 'disp'
[2.600000000000:imu_abstraction:STARTING_TASK] 'disp'
[2.600000000000:STATE] process "Inclinaison_Displayer", instance "disp", transition 2
[2.600000000000:ASSIGN] Assign pin_led_red new value: true
[2.600000000000:STATE] process "Inclinaison_Displayer", instance "disp", state "Main"
[2.600000000000:ASSIGN] Assign pin_led_green new value: false
[2.600000000000:ASSIGN] Assign pin_led_blue new value: false
[2.600000000000:imu_abstraction:TASK_FINISHED] 'disp'
[2.700000000000:imu_abstraction:STARTING_TASK] 'disp'
[2.700000000000:ASSIGN] Assign pin_led_green new value: false
[2.700000000000:ASSIGN] Assign pin_led_blue new value: false
[2.700000000000:imu_abstraction:TASK_FINISHED] 'disp'
[2.800000000000:imu_abstraction:STARTING_TASK] 'disp'
[2.800000000000:ASSIGN] Assign pin_led_green new value: false
[2.800000000000:ASSIGN] Assign pin_led_blue new value: false
[2.800000000000:imu_abstraction:TASK_FINISHED] 'disp'
[2.900000000000:imu_abstraction:STARTING_TASK] 'disp'
[2.900000000000:STATE] process "Inclinaison_Displayer", instance "disp", transition 2
[2.900000000000:ASSIGN] Assign pin_led_red new value: false
[2.900000000000:STATE] process "Inclinaison_Displayer", instance "disp", state "Main"
[2.900000000000:ASSIGN] Assign pin_led_green new value: false
[2.900000000000:ASSIGN] Assign pin_led_blue new value: false
[2.900000000000:imu_abstraction:TASK_FINISHED] 'disp'
[3.000000000000:imu_abstraction:STARTING_TASK] 'disp'
[3.000000000000:ASSIGN] Assign pin_led_green new value: false
[3.000000000000:ASSIGN] Assign pin_led_blue new value: false
[3.000000000000:imu_abstraction:TASK_FINISHED] 'disp'
EXIT (SUCCESS) AT 3s (3000000000000)

PID	start	end	name	state
0	0	0	disp	Init
0	100000000000	100000000000	disp	Offsetting
0	200000000000	200000000000	disp	Main
0	300000000000	300000000000	disp	Main
0	400000000000	400000000000	disp	Main
0	500000000000	500000000000	disp	Main
0	600000000000	600000000000	disp	Main
0	700000000000	700000000000	disp	Main
0	800000000000	800000000000	disp	Main
0	900000000000	900000000000	disp	Main
0	1000000000000	1000000000000	disp	Main
0	1100000000000	1100000000000	disp	Main
0	1200000000000	1200000000000	disp	Main
0	1300000000000	1300000000000	disp	Main
0	1400000000000	1400000000000	disp	Main
0	1500000000000	1500000000000	disp	Main
0	1600000000000	1600000000000	disp	Main
0	1700000000000	1700000000000	disp	Main
0	1800000000000	1800000000000	disp	Main
0	1900000000000	1900000000000	disp	Main
0	2000000000000	2000000000000	disp	Main
0	2100000000000	2100000000000	disp	Main
0	2200000000000	2200000000000	disp	Main
0	2300000000000	2300000000000	disp	Main
0	2400000000000	2400000000000	disp	Main
0	2500000000000	2500000000000	disp	Main
0	2600000000000	2600000000000	disp	Main
0	2700000000000	2700000000000	disp	Main
0	2800000000000	2800000000000	disp	Main
0	2900000000000	2900000000000	disp	Main
0	3000000000000	3000000000000	disp	Main