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@2:82c94d2c727a, 2016-08-23 (annotated)

Committer:
gakuburu
Date:
Tue Aug 23 05:30:07 2016 +0000
Revision:
2:82c94d2c727a
Parent:
1:e270b8b60070
Child:
3:9f80eee7aa78
bug 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 0:1b682aa7d935 4 twe_lite(tx, rx) {
gakuburu 0:1b682aa7d935 5 is_axis_initialized = false;
gakuburu 0:1b682aa7d935 6
gakuburu 0:1b682aa7d935 7 buffer_head = -1;
gakuburu 0:1b682aa7d935 8 twe_lite.baud(115200);
gakuburu 0:1b682aa7d935 9 twe_lite.attach(this, &LambdaController::data_receive_isr, Serial::RxIrq);
gakuburu 0:1b682aa7d935 10
gakuburu 0:1b682aa7d935 11 error_count = 0;
gakuburu 0:1b682aa7d935 12 error_id = NoDataError;
gakuburu 0:1b682aa7d935 13 error_detection_timer.attach(this, &LambdaController::error_detection_isr,
gakuburu 0:1b682aa7d935 14 0.03);
gakuburu 0:1b682aa7d935 15 }
gakuburu 0:1b682aa7d935 16
gakuburu 0:1b682aa7d935 17 void LambdaController::data_receive_isr() {
gakuburu 0:1b682aa7d935 18 int data;
gakuburu 0:1b682aa7d935 19 int i;
gakuburu 0:1b682aa7d935 20
gakuburu 0:1b682aa7d935 21 while (twe_lite.readable()) {
gakuburu 0:1b682aa7d935 22 data = twe_lite.getc();
gakuburu 0:1b682aa7d935 23
gakuburu 0:1b682aa7d935 24 if (data == 0x3A) {
gakuburu 0:1b682aa7d935 25 buffer_head = 0;
gakuburu 0:1b682aa7d935 26 }
gakuburu 0:1b682aa7d935 27
gakuburu 0:1b682aa7d935 28 if(data == EOF){
gakuburu 0:1b682aa7d935 29 buffer_head = -1;
gakuburu 0:1b682aa7d935 30 error_id = InvalidDataError;
gakuburu 2:82c94d2c727a 31 }
gakuburu 2:82c94d2c727a 32
gakuburu 2:82c94d2c727a 33 if (buffer_head < 10 && buffer_head != -1) {
gakuburu 2:82c94d2c727a 34 received_buffer[buffer_head] = data;
gakuburu 2:82c94d2c727a 35 buffer_head++;
gakuburu 2:82c94d2c727a 36 } else {
gakuburu 2:82c94d2c727a 37 error_id = InvalidDataError;
gakuburu 2:82c94d2c727a 38 buffer_head = -1;
gakuburu 2:82c94d2c727a 39 }
gakuburu 0:1b682aa7d935 40
gakuburu 0:1b682aa7d935 41 if (buffer_head == 10 && received_buffer[0] == 0x3A) {
gakuburu 0:1b682aa7d935 42 for (i = 0; i < 10; i++) {
gakuburu 0:1b682aa7d935 43 received_data[i] = received_buffer[i];
gakuburu 0:1b682aa7d935 44 }
gakuburu 0:1b682aa7d935 45
gakuburu 0:1b682aa7d935 46 if (!is_axis_initialized) {
gakuburu 0:1b682aa7d935 47 initialize_axis();
gakuburu 2:82c94d2c727a 48 is_axis_initialized = true;
gakuburu 0:1b682aa7d935 49 }
gakuburu 0:1b682aa7d935 50
gakuburu 0:1b682aa7d935 51 error_count = 0;
gakuburu 0:1b682aa7d935 52 error_id = NoError;
gakuburu 0:1b682aa7d935 53 } else {
gakuburu 0:1b682aa7d935 54 buffer_head = -1;
gakuburu 0:1b682aa7d935 55 error_id = InvalidDataError;
gakuburu 0:1b682aa7d935 56 }
gakuburu 0:1b682aa7d935 57 }
gakuburu 0:1b682aa7d935 58 }
gakuburu 0:1b682aa7d935 59
gakuburu 0:1b682aa7d935 60 void LambdaController::error_detection_isr() {
gakuburu 0:1b682aa7d935 61 if (error_count < error_threshold) {
gakuburu 0:1b682aa7d935 62 error_count++;
gakuburu 0:1b682aa7d935 63 } else {
gakuburu 0:1b682aa7d935 64 error_id = TimeoutError;
gakuburu 0:1b682aa7d935 65 }
gakuburu 0:1b682aa7d935 66 }
gakuburu 0:1b682aa7d935 67
gakuburu 0:1b682aa7d935 68 uint8_t LambdaController::get_all_switch() {
gakuburu 0:1b682aa7d935 69 return received_data[1];
gakuburu 0:1b682aa7d935 70 }
gakuburu 0:1b682aa7d935 71
gakuburu 0:1b682aa7d935 72 bool LambdaController::get_switch(int num) {
gakuburu 0:1b682aa7d935 73 bool status = false;
gakuburu 0:1b682aa7d935 74 if (num < 8 && num >= 0) {
gakuburu 0:1b682aa7d935 75 if (get_all_switch() & (1 << num)) {
gakuburu 0:1b682aa7d935 76 status = true;
gakuburu 0:1b682aa7d935 77 } else {
gakuburu 0:1b682aa7d935 78 status = false;
gakuburu 0:1b682aa7d935 79 }
gakuburu 0:1b682aa7d935 80 } else {
gakuburu 0:1b682aa7d935 81 status = false;
gakuburu 0:1b682aa7d935 82 }
gakuburu 0:1b682aa7d935 83
gakuburu 0:1b682aa7d935 84 return status;
gakuburu 0:1b682aa7d935 85 }
gakuburu 0:1b682aa7d935 86
gakuburu 0:1b682aa7d935 87 int LambdaController::get_raw_axis(AxisId id) {
gakuburu 1:e270b8b60070 88 uint8_t *data;
gakuburu 0:1b682aa7d935 89 int value;
gakuburu 0:1b682aa7d935 90
gakuburu 0:1b682aa7d935 91 data = &received_data[(int) id * 2 + 2];
gakuburu 0:1b682aa7d935 92 value = (data[0] << 8) | (data[1]);
gakuburu 0:1b682aa7d935 93
gakuburu 0:1b682aa7d935 94 return value;
gakuburu 0:1b682aa7d935 95 }
gakuburu 0:1b682aa7d935 96
gakuburu 0:1b682aa7d935 97 double LambdaController::get_axis(AxisId id) {
gakuburu 0:1b682aa7d935 98 double value = 0;
gakuburu 0:1b682aa7d935 99
gakuburu 0:1b682aa7d935 100 value = (double) (get_raw_axis(id) - axis_center[(int) id]) / 512.0;
gakuburu 0:1b682aa7d935 101
gakuburu 0:1b682aa7d935 102 if (value > -axis_threshold && value < axis_threshold) {
gakuburu 0:1b682aa7d935 103 value = 0.0;
gakuburu 0:1b682aa7d935 104 } else if (value < -1.0) {
gakuburu 0:1b682aa7d935 105 value = -1.0;
gakuburu 0:1b682aa7d935 106 } else if (value > 1.0) {
gakuburu 0:1b682aa7d935 107 value = 1;
gakuburu 0:1b682aa7d935 108 }
gakuburu 0:1b682aa7d935 109
gakuburu 0:1b682aa7d935 110 return value;
gakuburu 0:1b682aa7d935 111 }
gakuburu 0:1b682aa7d935 112
gakuburu 0:1b682aa7d935 113 void LambdaController::initialize_axis(void) {
gakuburu 0:1b682aa7d935 114 int i;
gakuburu 0:1b682aa7d935 115 for (i = 0; i < 4; i++) {
gakuburu 0:1b682aa7d935 116 initialize_axis((AxisId) i);
gakuburu 0:1b682aa7d935 117 }
gakuburu 0:1b682aa7d935 118 }
gakuburu 0:1b682aa7d935 119
gakuburu 0:1b682aa7d935 120 void LambdaController::initialize_axis(AxisId id) {
gakuburu 0:1b682aa7d935 121 axis_center[(int) id] = get_raw_axis(id);
gakuburu 0:1b682aa7d935 122 }
gakuburu 0:1b682aa7d935 123
gakuburu 0:1b682aa7d935 124 LambdaController::ErrorFactor LambdaController::get_error() {
gakuburu 0:1b682aa7d935 125 return error_id;
gakuburu 0:1b682aa7d935 126 }
gakuburu 0:1b682aa7d935 127
gakuburu 1:e270b8b60070 128 void LambdaController::debug(const char *format, ...)
gakuburu 0:1b682aa7d935 129 {
gakuburu 0:1b682aa7d935 130 va_list arg;
gakuburu 0:1b682aa7d935 131
gakuburu 0:1b682aa7d935 132 va_start(arg, format);
gakuburu 1:e270b8b60070 133 twe_lite.vprintf(format, arg);
gakuburu 0:1b682aa7d935 134 va_end(arg);
gakuburu 0:1b682aa7d935 135 }