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@0:1b682aa7d935, 2016-08-23 (annotated)

Committer:
gakuburu
Date:
Tue Aug 23 05:10:44 2016 +0000
Revision:
0:1b682aa7d935
Child:
1:e270b8b60070
new controller's programs

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 0:1b682aa7d935 31 }
gakuburu 0:1b682aa7d935 32
gakuburu 0:1b682aa7d935 33 if (buffer_head == 10 && received_buffer[0] == 0x3A) {
gakuburu 0:1b682aa7d935 34 for (i = 0; i < 10; i++) {
gakuburu 0:1b682aa7d935 35 received_data[i] = received_buffer[i];
gakuburu 0:1b682aa7d935 36 }
gakuburu 0:1b682aa7d935 37
gakuburu 0:1b682aa7d935 38 if (!is_axis_initialized) {
gakuburu 0:1b682aa7d935 39 initialize_axis();
gakuburu 0:1b682aa7d935 40 }
gakuburu 0:1b682aa7d935 41
gakuburu 0:1b682aa7d935 42 error_count = 0;
gakuburu 0:1b682aa7d935 43 error_id = NoError;
gakuburu 0:1b682aa7d935 44 } else {
gakuburu 0:1b682aa7d935 45 buffer_head = -1;
gakuburu 0:1b682aa7d935 46 error_id = InvalidDataError;
gakuburu 0:1b682aa7d935 47 }
gakuburu 0:1b682aa7d935 48 }
gakuburu 0:1b682aa7d935 49 }
gakuburu 0:1b682aa7d935 50
gakuburu 0:1b682aa7d935 51 void LambdaController::error_detection_isr() {
gakuburu 0:1b682aa7d935 52 if (error_count < error_threshold) {
gakuburu 0:1b682aa7d935 53 error_count++;
gakuburu 0:1b682aa7d935 54 } else {
gakuburu 0:1b682aa7d935 55 error_id = TimeoutError;
gakuburu 0:1b682aa7d935 56 }
gakuburu 0:1b682aa7d935 57 }
gakuburu 0:1b682aa7d935 58
gakuburu 0:1b682aa7d935 59 uint8_t LambdaController::get_all_switch() {
gakuburu 0:1b682aa7d935 60 return received_data[1];
gakuburu 0:1b682aa7d935 61 }
gakuburu 0:1b682aa7d935 62
gakuburu 0:1b682aa7d935 63 bool LambdaController::get_switch(int num) {
gakuburu 0:1b682aa7d935 64 bool status = false;
gakuburu 0:1b682aa7d935 65 if (num < 8 && num >= 0) {
gakuburu 0:1b682aa7d935 66 if (get_all_switch() & (1 << num)) {
gakuburu 0:1b682aa7d935 67 status = true;
gakuburu 0:1b682aa7d935 68 } else {
gakuburu 0:1b682aa7d935 69 status = false;
gakuburu 0:1b682aa7d935 70 }
gakuburu 0:1b682aa7d935 71 } else {
gakuburu 0:1b682aa7d935 72 status = false;
gakuburu 0:1b682aa7d935 73 }
gakuburu 0:1b682aa7d935 74
gakuburu 0:1b682aa7d935 75 return status;
gakuburu 0:1b682aa7d935 76 }
gakuburu 0:1b682aa7d935 77
gakuburu 0:1b682aa7d935 78 int LambdaController::get_raw_axis(AxisId id) {
gakuburu 0:1b682aa7d935 79 int *data;
gakuburu 0:1b682aa7d935 80 int value;
gakuburu 0:1b682aa7d935 81
gakuburu 0:1b682aa7d935 82 data = &received_data[(int) id * 2 + 2];
gakuburu 0:1b682aa7d935 83 value = (data[0] << 8) | (data[1]);
gakuburu 0:1b682aa7d935 84
gakuburu 0:1b682aa7d935 85 return value;
gakuburu 0:1b682aa7d935 86 }
gakuburu 0:1b682aa7d935 87
gakuburu 0:1b682aa7d935 88 double LambdaController::get_axis(AxisId id) {
gakuburu 0:1b682aa7d935 89 double value = 0;
gakuburu 0:1b682aa7d935 90
gakuburu 0:1b682aa7d935 91 value = (double) (get_raw_axis(id) - axis_center[(int) id]) / 512.0;
gakuburu 0:1b682aa7d935 92
gakuburu 0:1b682aa7d935 93 if (value > -axis_threshold && value < axis_threshold) {
gakuburu 0:1b682aa7d935 94 value = 0.0;
gakuburu 0:1b682aa7d935 95 } else if (value < -1.0) {
gakuburu 0:1b682aa7d935 96 value = -1.0;
gakuburu 0:1b682aa7d935 97 } else if (value > 1.0) {
gakuburu 0:1b682aa7d935 98 value = 1;
gakuburu 0:1b682aa7d935 99 }
gakuburu 0:1b682aa7d935 100
gakuburu 0:1b682aa7d935 101 return value;
gakuburu 0:1b682aa7d935 102 }
gakuburu 0:1b682aa7d935 103
gakuburu 0:1b682aa7d935 104 void LambdaController::initialize_axis(void) {
gakuburu 0:1b682aa7d935 105 int i;
gakuburu 0:1b682aa7d935 106 for (i = 0; i < 4; i++) {
gakuburu 0:1b682aa7d935 107 initialize_axis((AxisId) i);
gakuburu 0:1b682aa7d935 108 }
gakuburu 0:1b682aa7d935 109 }
gakuburu 0:1b682aa7d935 110
gakuburu 0:1b682aa7d935 111 void LambdaController::initialize_axis(AxisId id) {
gakuburu 0:1b682aa7d935 112 axis_center[(int) id] = get_raw_axis(id);
gakuburu 0:1b682aa7d935 113 }
gakuburu 0:1b682aa7d935 114
gakuburu 0:1b682aa7d935 115 LambdaController::ErrorFactor LambdaController::get_error() {
gakuburu 0:1b682aa7d935 116 return error_id;
gakuburu 0:1b682aa7d935 117 }
gakuburu 0:1b682aa7d935 118
gakuburu 0:1b682aa7d935 119 void GetCon::debug(const char *format, ...)
gakuburu 0:1b682aa7d935 120 {
gakuburu 0:1b682aa7d935 121 va_list arg;
gakuburu 0:1b682aa7d935 122
gakuburu 0:1b682aa7d935 123 va_start(arg, format);
gakuburu 0:1b682aa7d935 124 device.vprintf(format, arg);
gakuburu 0:1b682aa7d935 125 va_end(arg);
gakuburu 0:1b682aa7d935 126 }