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@3:9f80eee7aa78, 2016-08-23 (annotated)

Committer:
gakuburu
Date:
Tue Aug 23 08:30:41 2016 +0000
Revision:
3:9f80eee7aa78
Parent:
2:82c94d2c727a
Child:
4:c35227d14896
bugs 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 3:9f80eee7aa78 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 3:9f80eee7aa78 56
gakuburu 3:9f80eee7aa78 57 if(data == EOF) {
gakuburu 3:9f80eee7aa78 58 error_id = SerialBusyError;
gakuburu 3:9f80eee7aa78 59 break;
gakuburu 3:9f80eee7aa78 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 3:9f80eee7aa78 99 data = &received_data[(int) id * 2 + 2];
gakuburu 3:9f80eee7aa78 100 value = (data[0] << 8) | (data[1]);
gakuburu 0:1b682aa7d935 101
gakuburu 3:9f80eee7aa78 102 return value;
gakuburu 0:1b682aa7d935 103 }
gakuburu 0:1b682aa7d935 104
gakuburu 3:9f80eee7aa78 105 double LambdaController::get_axis(AxisId id)
gakuburu 3:9f80eee7aa78 106 {
gakuburu 3:9f80eee7aa78 107 double value = 0;
gakuburu 0:1b682aa7d935 108
gakuburu 3:9f80eee7aa78 109 value = (double) (get_raw_axis(id) - axis_center[(int) id]) / 512.0;
gakuburu 0:1b682aa7d935 110
gakuburu 3:9f80eee7aa78 111 if (value > -axis_threshold && value < axis_threshold) {
gakuburu 3:9f80eee7aa78 112 value = 0.0;
gakuburu 3:9f80eee7aa78 113 } else if (value < -1.0) {
gakuburu 3:9f80eee7aa78 114 value = -1.0;
gakuburu 3:9f80eee7aa78 115 } else if (value > 1.0) {
gakuburu 3:9f80eee7aa78 116 value = 1;
gakuburu 3:9f80eee7aa78 117 }
gakuburu 0:1b682aa7d935 118
gakuburu 3:9f80eee7aa78 119 return value;
gakuburu 0:1b682aa7d935 120 }
gakuburu 0:1b682aa7d935 121
gakuburu 3:9f80eee7aa78 122 void LambdaController::initialize_axis(void)
gakuburu 3:9f80eee7aa78 123 {
gakuburu 3:9f80eee7aa78 124 int i;
gakuburu 3:9f80eee7aa78 125 for (i = 0; i < 4; i++) {
gakuburu 3:9f80eee7aa78 126 initialize_axis((AxisId) i);
gakuburu 3:9f80eee7aa78 127 }
gakuburu 0:1b682aa7d935 128 }
gakuburu 0:1b682aa7d935 129
gakuburu 3:9f80eee7aa78 130 void LambdaController::initialize_axis(AxisId id)
gakuburu 3:9f80eee7aa78 131 {
gakuburu 3:9f80eee7aa78 132 axis_center[(int) id] = get_raw_axis(id);
gakuburu 0:1b682aa7d935 133 }
gakuburu 0:1b682aa7d935 134
gakuburu 3:9f80eee7aa78 135 LambdaController::ErrorFactor LambdaController::get_error()
gakuburu 3:9f80eee7aa78 136 {
gakuburu 3:9f80eee7aa78 137 return error_id;
gakuburu 0:1b682aa7d935 138 }
gakuburu 0:1b682aa7d935 139
gakuburu 1:e270b8b60070 140 void LambdaController::debug(const char *format, ...)
gakuburu 0:1b682aa7d935 141 {
gakuburu 3:9f80eee7aa78 142 /*
gakuburu 0:1b682aa7d935 143 va_list arg;
gakuburu 0:1b682aa7d935 144
gakuburu 0:1b682aa7d935 145 va_start(arg, format);
gakuburu 1:e270b8b60070 146 twe_lite.vprintf(format, arg);
gakuburu 0:1b682aa7d935 147 va_end(arg);
gakuburu 3:9f80eee7aa78 148 */
gakuburu 3:9f80eee7aa78 149 }