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@4:c35227d14896, 2016-08-24 (annotated)

Committer:
gakuburu
Date:
Wed Aug 24 01:35:09 2016 +0000
Revision:
4:c35227d14896
Parent:
3:9f80eee7aa78
Child:
5:57c6d5db3d52
error fixed

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 3:9f80eee7aa78 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 3:9f80eee7aa78 14 error_detection_timer.attach(this, &LambdaController::error_detection_isr,
gakuburu 3:9f80eee7aa78 15 timeout_time);
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 3:9f80eee7aa78 51 } else {
gakuburu 3:9f80eee7aa78 52 buffer_head = -1;
gakuburu 3:9f80eee7aa78 53 error_id = InvalidDataError;
gakuburu 3:9f80eee7aa78 54 }
gakuburu 3:9f80eee7aa78 55 }
gakuburu 4:c35227d14896 56
gakuburu 3:9f80eee7aa78 57 if(data == EOF) {
gakuburu 3:9f80eee7aa78 58 error_id = SerialBusyError;
gakuburu 3:9f80eee7aa78 59 break;
gakuburu 4:c35227d14896 60 }
gakuburu 3:9f80eee7aa78 61 }
gakuburu 0:1b682aa7d935 62 }
gakuburu 0:1b682aa7d935 63
gakuburu 3:9f80eee7aa78 64 void LambdaController::error_detection_isr()
gakuburu 3:9f80eee7aa78 65 {
gakuburu 3:9f80eee7aa78 66 if (error_count < error_threshold) {
gakuburu 3:9f80eee7aa78 67 error_count++;
gakuburu 3:9f80eee7aa78 68 } else {
gakuburu 3:9f80eee7aa78 69 error_id = TimeoutError;
gakuburu 3:9f80eee7aa78 70 }
gakuburu 0:1b682aa7d935 71 }
gakuburu 0:1b682aa7d935 72
gakuburu 3:9f80eee7aa78 73 uint8_t LambdaController::get_all_switch()
gakuburu 3:9f80eee7aa78 74 {
gakuburu 3:9f80eee7aa78 75 return received_data[1];
gakuburu 0:1b682aa7d935 76 }
gakuburu 0:1b682aa7d935 77
gakuburu 3:9f80eee7aa78 78 bool LambdaController::get_switch(int num)
gakuburu 3:9f80eee7aa78 79 {
gakuburu 3:9f80eee7aa78 80 bool status = false;
gakuburu 3:9f80eee7aa78 81 if (num < 8 && num >= 0) {
gakuburu 3:9f80eee7aa78 82 if (get_all_switch() & (1 << num)) {
gakuburu 3:9f80eee7aa78 83 status = true;
gakuburu 3:9f80eee7aa78 84 } else {
gakuburu 3:9f80eee7aa78 85 status = false;
gakuburu 3:9f80eee7aa78 86 }
gakuburu 3:9f80eee7aa78 87 } else {
gakuburu 3:9f80eee7aa78 88 status = false;
gakuburu 3:9f80eee7aa78 89 }
gakuburu 0:1b682aa7d935 90
gakuburu 3:9f80eee7aa78 91 return status;
gakuburu 0:1b682aa7d935 92 }
gakuburu 0:1b682aa7d935 93
gakuburu 3:9f80eee7aa78 94 int LambdaController::get_raw_axis(AxisId id)
gakuburu 3:9f80eee7aa78 95 {
gakuburu 3:9f80eee7aa78 96 uint8_t *data;
gakuburu 3:9f80eee7aa78 97 int value;
gakuburu 0:1b682aa7d935 98
gakuburu 4:c35227d14896 99 if((int)id < 4 && (int)id >= 0) {
gakuburu 4:c35227d14896 100 data = &received_data[(int) id * 2 + 2];
gakuburu 4:c35227d14896 101 value = (data[0] << 8) | (data[1]);
gakuburu 4:c35227d14896 102 } else {
gakuburu 4:c35227d14896 103 value = -1;
gakuburu 4:c35227d14896 104 }
gakuburu 0:1b682aa7d935 105
gakuburu 3:9f80eee7aa78 106 return value;
gakuburu 0:1b682aa7d935 107 }
gakuburu 0:1b682aa7d935 108
gakuburu 3:9f80eee7aa78 109 double LambdaController::get_axis(AxisId id)
gakuburu 3:9f80eee7aa78 110 {
gakuburu 3:9f80eee7aa78 111 double value = 0;
gakuburu 4:c35227d14896 112 int raw;
gakuburu 4:c35227d14896 113 raw = get_raw_axis(id);
gakuburu 0:1b682aa7d935 114
gakuburu 4:c35227d14896 115 if(raw != -1) {
gakuburu 4:c35227d14896 116 value = (double) (raw - axis_center[(int) id]) / 512.0;
gakuburu 0:1b682aa7d935 117
gakuburu 4:c35227d14896 118 if (value > -axis_threshold && value < axis_threshold) {
gakuburu 4:c35227d14896 119 value = 0.0;
gakuburu 4:c35227d14896 120 } else if (value < -1.0) {
gakuburu 4:c35227d14896 121 value = -1.0;
gakuburu 4:c35227d14896 122 } else if (value > 1.0) {
gakuburu 4:c35227d14896 123 value = 1;
gakuburu 4:c35227d14896 124 }
gakuburu 4:c35227d14896 125 } else {
gakuburu 3:9f80eee7aa78 126 value = 0.0;
gakuburu 3:9f80eee7aa78 127 }
gakuburu 0:1b682aa7d935 128
gakuburu 3:9f80eee7aa78 129 return value;
gakuburu 0:1b682aa7d935 130 }
gakuburu 0:1b682aa7d935 131
gakuburu 3:9f80eee7aa78 132 void LambdaController::initialize_axis(void)
gakuburu 3:9f80eee7aa78 133 {
gakuburu 3:9f80eee7aa78 134 int i;
gakuburu 3:9f80eee7aa78 135 for (i = 0; i < 4; i++) {
gakuburu 3:9f80eee7aa78 136 initialize_axis((AxisId) i);
gakuburu 3:9f80eee7aa78 137 }
gakuburu 0:1b682aa7d935 138 }
gakuburu 0:1b682aa7d935 139
gakuburu 3:9f80eee7aa78 140 void LambdaController::initialize_axis(AxisId id)
gakuburu 3:9f80eee7aa78 141 {
gakuburu 4:c35227d14896 142 int raw;
gakuburu 4:c35227d14896 143 raw = get_raw_axis(id);
gakuburu 4:c35227d14896 144
gakuburu 4:c35227d14896 145 if(raw != -1) {
gakuburu 4:c35227d14896 146 axis_center[(int) id] = raw;
gakuburu 4:c35227d14896 147 }
gakuburu 0:1b682aa7d935 148 }
gakuburu 0:1b682aa7d935 149
gakuburu 3:9f80eee7aa78 150 LambdaController::ErrorFactor LambdaController::get_error()
gakuburu 3:9f80eee7aa78 151 {
gakuburu 3:9f80eee7aa78 152 return error_id;
gakuburu 0:1b682aa7d935 153 }
gakuburu 0:1b682aa7d935 154
gakuburu 1:e270b8b60070 155 void LambdaController::debug(const char *format, ...)
gakuburu 0:1b682aa7d935 156 {
gakuburu 3:9f80eee7aa78 157 /*
gakuburu 0:1b682aa7d935 158 va_list arg;
gakuburu 0:1b682aa7d935 159
gakuburu 0:1b682aa7d935 160 va_start(arg, format);
gakuburu 1:e270b8b60070 161 twe_lite.vprintf(format, arg);
gakuburu 0:1b682aa7d935 162 va_end(arg);
gakuburu 4:c35227d14896 163 */
gakuburu 3:9f80eee7aa78 164 }
gakuburu 4:c35227d14896 165