takeshi yoshida / Mbed 2 deprecated 2016_lambdaController

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Dependents:  

LambdaController.cpp@5:57c6d5db3d52, 2016-08-29 (annotated)

Committer:
gakuburu
Date:
Mon Aug 29 08:19:17 2016 +0000
Revision:
5:57c6d5db3d52
Parent:
4:c35227d14896
Child:
6:c4e901c6001a
Ticker Abolition.; Timer Implementation.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gakuburu 0:1b682aa7d935 1 #include "LambdaController.h"
gakuburu 0:1b682aa7d935 2
gakuburu 0:1b682aa7d935 3 LambdaController::LambdaController(PinName tx, PinName rx) :
gakuburu 3:9f80eee7aa78 4 twe_lite(tx, rx)
gakuburu 5:57c6d5db3d52 5 {
gakuburu 3:9f80eee7aa78 6 is_axis_initialized = false;
gakuburu 0:1b682aa7d935 7
gakuburu 3:9f80eee7aa78 8 buffer_head = -1;
gakuburu 3:9f80eee7aa78 9 twe_lite.baud(115200);
gakuburu 3:9f80eee7aa78 10 twe_lite.attach(this, &LambdaController::data_receive_isr, Serial::RxIrq);
gakuburu 0:1b682aa7d935 11
gakuburu 3:9f80eee7aa78 12 error_count = 0;
gakuburu 3:9f80eee7aa78 13 error_id = NoDataError;
gakuburu 5:57c6d5db3d52 14
gakuburu 5:57c6d5db3d52 15 error_timer.start();
gakuburu 0:1b682aa7d935 16 }
gakuburu 0:1b682aa7d935 17
gakuburu 3:9f80eee7aa78 18 void LambdaController::data_receive_isr()
gakuburu 3:9f80eee7aa78 19 {
gakuburu 3:9f80eee7aa78 20 int data;
gakuburu 3:9f80eee7aa78 21 int i;
gakuburu 0:1b682aa7d935 22
gakuburu 3:9f80eee7aa78 23 while (twe_lite.readable()) {
gakuburu 3:9f80eee7aa78 24 data = twe_lite.getc();
gakuburu 4:c35227d14896 25
gakuburu 3:9f80eee7aa78 26 if (data == 0x3A) {
gakuburu 3:9f80eee7aa78 27 buffer_head = 0;
gakuburu 3:9f80eee7aa78 28 }
gakuburu 3:9f80eee7aa78 29
gakuburu 3:9f80eee7aa78 30 if (buffer_head < 10 && buffer_head != -1) {
gakuburu 3:9f80eee7aa78 31 received_buffer[buffer_head] = data;
gakuburu 3:9f80eee7aa78 32 buffer_head++;
gakuburu 3:9f80eee7aa78 33 } else {
gakuburu 3:9f80eee7aa78 34 error_id = InvalidDataError;
gakuburu 3:9f80eee7aa78 35 buffer_head = -1;
gakuburu 3:9f80eee7aa78 36 }
gakuburu 0:1b682aa7d935 37
gakuburu 3:9f80eee7aa78 38 if (buffer_head == 10) {
gakuburu 3:9f80eee7aa78 39 if(received_buffer[0] == 0x3A) {
gakuburu 3:9f80eee7aa78 40 for (i = 0; i < 10; i++) {
gakuburu 3:9f80eee7aa78 41 received_data[i] = received_buffer[i];
gakuburu 3:9f80eee7aa78 42 }
gakuburu 3:9f80eee7aa78 43
gakuburu 3:9f80eee7aa78 44 if (!is_axis_initialized) {
gakuburu 3:9f80eee7aa78 45 initialize_axis();
gakuburu 3:9f80eee7aa78 46 is_axis_initialized = true;
gakuburu 3:9f80eee7aa78 47 }
gakuburu 0:1b682aa7d935 48
gakuburu 3:9f80eee7aa78 49 error_count = 0;
gakuburu 3:9f80eee7aa78 50 error_id = NoError;
gakuburu 5:57c6d5db3d52 51 error_timer.reset();
gakuburu 3:9f80eee7aa78 52 } else {
gakuburu 3:9f80eee7aa78 53 buffer_head = -1;
gakuburu 3:9f80eee7aa78 54 error_id = InvalidDataError;
gakuburu 3:9f80eee7aa78 55 }
gakuburu 3:9f80eee7aa78 56 }
gakuburu 4:c35227d14896 57
gakuburu 3:9f80eee7aa78 58 if(data == EOF) {
gakuburu 3:9f80eee7aa78 59 error_id = SerialBusyError;
gakuburu 3:9f80eee7aa78 60 break;
gakuburu 4:c35227d14896 61 }
gakuburu 3:9f80eee7aa78 62 }
gakuburu 0:1b682aa7d935 63 }
gakuburu 0:1b682aa7d935 64
gakuburu 3:9f80eee7aa78 65 uint8_t LambdaController::get_all_switch()
gakuburu 3:9f80eee7aa78 66 {
gakuburu 3:9f80eee7aa78 67 return received_data[1];
gakuburu 0:1b682aa7d935 68 }
gakuburu 0:1b682aa7d935 69
gakuburu 3:9f80eee7aa78 70 bool LambdaController::get_switch(int num)
gakuburu 3:9f80eee7aa78 71 {
gakuburu 3:9f80eee7aa78 72 bool status = false;
gakuburu 3:9f80eee7aa78 73 if (num < 8 && num >= 0) {
gakuburu 3:9f80eee7aa78 74 if (get_all_switch() & (1 << num)) {
gakuburu 3:9f80eee7aa78 75 status = true;
gakuburu 3:9f80eee7aa78 76 } else {
gakuburu 3:9f80eee7aa78 77 status = false;
gakuburu 3:9f80eee7aa78 78 }
gakuburu 3:9f80eee7aa78 79 } else {
gakuburu 3:9f80eee7aa78 80 status = false;
gakuburu 3:9f80eee7aa78 81 }
gakuburu 0:1b682aa7d935 82
gakuburu 3:9f80eee7aa78 83 return status;
gakuburu 0:1b682aa7d935 84 }
gakuburu 0:1b682aa7d935 85
gakuburu 3:9f80eee7aa78 86 int LambdaController::get_raw_axis(AxisId id)
gakuburu 3:9f80eee7aa78 87 {
gakuburu 3:9f80eee7aa78 88 uint8_t *data;
gakuburu 3:9f80eee7aa78 89 int value;
gakuburu 0:1b682aa7d935 90
gakuburu 4:c35227d14896 91 if((int)id < 4 && (int)id >= 0) {
gakuburu 4:c35227d14896 92 data = &received_data[(int) id * 2 + 2];
gakuburu 4:c35227d14896 93 value = (data[0] << 8) | (data[1]);
gakuburu 4:c35227d14896 94 } else {
gakuburu 4:c35227d14896 95 value = -1;
gakuburu 4:c35227d14896 96 }
gakuburu 0:1b682aa7d935 97
gakuburu 3:9f80eee7aa78 98 return value;
gakuburu 0:1b682aa7d935 99 }
gakuburu 0:1b682aa7d935 100
gakuburu 3:9f80eee7aa78 101 double LambdaController::get_axis(AxisId id)
gakuburu 3:9f80eee7aa78 102 {
gakuburu 3:9f80eee7aa78 103 double value = 0;
gakuburu 4:c35227d14896 104 int raw;
gakuburu 4:c35227d14896 105 raw = get_raw_axis(id);
gakuburu 0:1b682aa7d935 106
gakuburu 4:c35227d14896 107 if(raw != -1) {
gakuburu 4:c35227d14896 108 value = (double) (raw - axis_center[(int) id]) / 512.0;
gakuburu 0:1b682aa7d935 109
gakuburu 4:c35227d14896 110 if (value > -axis_threshold && value < axis_threshold) {
gakuburu 4:c35227d14896 111 value = 0.0;
gakuburu 4:c35227d14896 112 } else if (value < -1.0) {
gakuburu 4:c35227d14896 113 value = -1.0;
gakuburu 4:c35227d14896 114 } else if (value > 1.0) {
gakuburu 4:c35227d14896 115 value = 1;
gakuburu 4:c35227d14896 116 }
gakuburu 4:c35227d14896 117 } else {
gakuburu 3:9f80eee7aa78 118 value = 0.0;
gakuburu 3:9f80eee7aa78 119 }
gakuburu 0:1b682aa7d935 120
gakuburu 3:9f80eee7aa78 121 return value;
gakuburu 0:1b682aa7d935 122 }
gakuburu 0:1b682aa7d935 123
gakuburu 3:9f80eee7aa78 124 void LambdaController::initialize_axis(void)
gakuburu 3:9f80eee7aa78 125 {
gakuburu 3:9f80eee7aa78 126 int i;
gakuburu 3:9f80eee7aa78 127 for (i = 0; i < 4; i++) {
gakuburu 3:9f80eee7aa78 128 initialize_axis((AxisId) i);
gakuburu 3:9f80eee7aa78 129 }
gakuburu 0:1b682aa7d935 130 }
gakuburu 0:1b682aa7d935 131
gakuburu 3:9f80eee7aa78 132 void LambdaController::initialize_axis(AxisId id)
gakuburu 3:9f80eee7aa78 133 {
gakuburu 4:c35227d14896 134 int raw;
gakuburu 4:c35227d14896 135 raw = get_raw_axis(id);
gakuburu 4:c35227d14896 136
gakuburu 4:c35227d14896 137 if(raw != -1) {
gakuburu 4:c35227d14896 138 axis_center[(int) id] = raw;
gakuburu 4:c35227d14896 139 }
gakuburu 0:1b682aa7d935 140 }
gakuburu 0:1b682aa7d935 141
gakuburu 3:9f80eee7aa78 142 LambdaController::ErrorFactor LambdaController::get_error()
gakuburu 3:9f80eee7aa78 143 {
gakuburu 5:57c6d5db3d52 144 if (error_timer >= timeout_time) error_id = TimeoutError;
gakuburu 3:9f80eee7aa78 145 return error_id;
gakuburu 5:57c6d5db3d52 146 }