takeshi yoshida / Mbed 2 deprecated 2016_lambda_Controller

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

LambdaController.cpp@2:cf08273cbd94, 2016-11-13 (annotated)

Committer:
gakuburu
Date:
Sun Nov 13 01:59:49 2016 +0000
Revision:
2:cf08273cbd94
Parent:
1:3785af7e715e 
nakamura ganbatta

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gakuburu 0:728c138f0486 1 #include "LambdaController.h"
gakuburu 0:728c138f0486 2
gakuburu 0:728c138f0486 3 LambdaController::LambdaController(PinName tx, PinName rx, PinName hzpin) :
gakuburu 0:728c138f0486 4 device(tx, rx), hzjudge(hzpin)
gakuburu 0:728c138f0486 5 {
gakuburu 0:728c138f0486 6 is_axis_initialized = false;
gakuburu 0:728c138f0486 7
gakuburu 0:728c138f0486 8 buffer_head = -1;
gakuburu 0:728c138f0486 9 device.baud(19200);
gakuburu 0:728c138f0486 10 device.attach(this, &LambdaController::data_receive_isr, Serial::RxIrq);
gakuburu 0:728c138f0486 11
gakuburu 0:728c138f0486 12 hzjudge.mode(PullUp);
gakuburu 0:728c138f0486 13
gakuburu 0:728c138f0486 14 error_count = 0;
gakuburu 0:728c138f0486 15 error_id = NoDataError;
gakuburu 0:728c138f0486 16
gakuburu 0:728c138f0486 17 error_timer.start();
gakuburu 0:728c138f0486 18 }
gakuburu 0:728c138f0486 19
gakuburu 0:728c138f0486 20 void LambdaController::data_receive_isr()
gakuburu 0:728c138f0486 21 {
gakuburu 0:728c138f0486 22 char data, *ptr;
gakuburu 0:728c138f0486 23 int i;
gakuburu 0:728c138f0486 24
gakuburu 0:728c138f0486 25 while (device.readable())
gakuburu 0:728c138f0486 26 {
gakuburu 0:728c138f0486 27 data = device.getc();
gakuburu 0:728c138f0486 28
gakuburu 0:728c138f0486 29 if (data == 0x3A)
gakuburu 0:728c138f0486 30 {
gakuburu 0:728c138f0486 31 buffer_head = 0;
gakuburu 0:728c138f0486 32 }
gakuburu 0:728c138f0486 33
gakuburu 0:728c138f0486 34 if (hzjudge)
gakuburu 0:728c138f0486 35 {
gakuburu 0:728c138f0486 36 if (buffer_head < 21 && buffer_head != -1)
gakuburu 0:728c138f0486 37 {
gakuburu 0:728c138f0486 38 if (data != ',')
gakuburu 0:728c138f0486 39 {
gakuburu 0:728c138f0486 40 if (buffer_head == 0)
gakuburu 0:728c138f0486 41 {
gakuburu 0:728c138f0486 42 received_buffer[buffer_head] = 0x3A;
gakuburu 0:728c138f0486 43 }
gakuburu 0:728c138f0486 44 else
gakuburu 0:728c138f0486 45 {
gakuburu 0:728c138f0486 46 if (buffer_head % 2 == 1)
gakuburu 0:728c138f0486 47 {
gakuburu 0:728c138f0486 48 received_buffer[buffer_head / 2 + 1] = strtol(&data, &ptr, 16) << 4;
gakuburu 0:728c138f0486 49 }
gakuburu 0:728c138f0486 50 else if (buffer_head % 2 == 0)
gakuburu 0:728c138f0486 51 {
gakuburu 0:728c138f0486 52 received_buffer[buffer_head / 2] |= strtol(&data, &ptr, 16);
gakuburu 0:728c138f0486 53 }
gakuburu 0:728c138f0486 54 }
gakuburu 0:728c138f0486 55 buffer_head++;
gakuburu 0:728c138f0486 56 }
gakuburu 0:728c138f0486 57 }
gakuburu 0:728c138f0486 58
gakuburu 0:728c138f0486 59 if (buffer_head == 21)
gakuburu 0:728c138f0486 60 {
gakuburu 0:728c138f0486 61 if (received_buffer[0] == 0x3A)
gakuburu 0:728c138f0486 62 {
gakuburu 0:728c138f0486 63 for (i = 0; i < 11; i++)
gakuburu 0:728c138f0486 64 {
gakuburu 0:728c138f0486 65 received_data[i] = received_buffer[i];
gakuburu 0:728c138f0486 66 }
gakuburu 0:728c138f0486 67
gakuburu 0:728c138f0486 68 if (!is_axis_initialized)
gakuburu 0:728c138f0486 69 {
gakuburu 0:728c138f0486 70 initialize_axis();
gakuburu 0:728c138f0486 71 is_axis_initialized = true;
gakuburu 0:728c138f0486 72 }
gakuburu 0:728c138f0486 73
gakuburu 0:728c138f0486 74 error_count = 0;
gakuburu 0:728c138f0486 75 error_id = NoError;
gakuburu 0:728c138f0486 76 error_timer.reset();
gakuburu 0:728c138f0486 77 }
gakuburu 0:728c138f0486 78 else
gakuburu 0:728c138f0486 79 {
gakuburu 0:728c138f0486 80 buffer_head = -1;
gakuburu 0:728c138f0486 81 error_id = InvalidDataError;
gakuburu 0:728c138f0486 82 }
gakuburu 0:728c138f0486 83 }
gakuburu 0:728c138f0486 84 }
gakuburu 0:728c138f0486 85 else
gakuburu 0:728c138f0486 86 {
gakuburu 0:728c138f0486 87 if (buffer_head < 11 && buffer_head != -1)
gakuburu 0:728c138f0486 88 {
gakuburu 0:728c138f0486 89 received_buffer[buffer_head] = data;
gakuburu 0:728c138f0486 90 buffer_head++;
gakuburu 0:728c138f0486 91 }
gakuburu 0:728c138f0486 92 else
gakuburu 0:728c138f0486 93 {
gakuburu 1:3785af7e715e 94 //error_id = InvalidDataError;
gakuburu 0:728c138f0486 95 buffer_head = -1;
gakuburu 0:728c138f0486 96 }
gakuburu 0:728c138f0486 97
gakuburu 0:728c138f0486 98 if (buffer_head == 11)
gakuburu 0:728c138f0486 99 {
gakuburu 0:728c138f0486 100 if(received_buffer[0] == 0x3A)
gakuburu 0:728c138f0486 101 {
gakuburu 0:728c138f0486 102 for (i = 0; i < 11; i++)
gakuburu 0:728c138f0486 103 {
gakuburu 0:728c138f0486 104 received_data[i] = received_buffer[i];
gakuburu 0:728c138f0486 105 }
gakuburu 0:728c138f0486 106
gakuburu 0:728c138f0486 107 if (!is_axis_initialized)
gakuburu 0:728c138f0486 108 {
gakuburu 0:728c138f0486 109 initialize_axis();
gakuburu 0:728c138f0486 110 is_axis_initialized = true;
gakuburu 0:728c138f0486 111 }
gakuburu 0:728c138f0486 112
gakuburu 0:728c138f0486 113 error_count = 0;
gakuburu 0:728c138f0486 114 error_id = NoError;
gakuburu 0:728c138f0486 115 error_timer.reset();
gakuburu 0:728c138f0486 116 }
gakuburu 0:728c138f0486 117 else
gakuburu 0:728c138f0486 118 {
gakuburu 0:728c138f0486 119 buffer_head = -1;
gakuburu 0:728c138f0486 120 error_id = InvalidDataError;
gakuburu 0:728c138f0486 121 }
gakuburu 0:728c138f0486 122 }
gakuburu 0:728c138f0486 123 }
gakuburu 0:728c138f0486 124
gakuburu 0:728c138f0486 125 if(data == EOF)
gakuburu 0:728c138f0486 126 {
gakuburu 0:728c138f0486 127 error_id = SerialBusyError;
gakuburu 0:728c138f0486 128 break;
gakuburu 0:728c138f0486 129 }
gakuburu 0:728c138f0486 130 }
gakuburu 0:728c138f0486 131 }
gakuburu 0:728c138f0486 132
gakuburu 0:728c138f0486 133 uint16_t LambdaController::get_all_switch()
gakuburu 0:728c138f0486 134 {
gakuburu 0:728c138f0486 135 return (received_data[1] << 8) | received_data[2];
gakuburu 0:728c138f0486 136 }
gakuburu 0:728c138f0486 137
gakuburu 0:728c138f0486 138 bool LambdaController::get_switch(int num)
gakuburu 0:728c138f0486 139 {
gakuburu 0:728c138f0486 140 bool status = false;
gakuburu 0:728c138f0486 141 if (num < 11 && num >= 0)
gakuburu 0:728c138f0486 142 {
gakuburu 0:728c138f0486 143 if (get_all_switch() & (1 << num))
gakuburu 0:728c138f0486 144 {
gakuburu 0:728c138f0486 145 status = true;
gakuburu 0:728c138f0486 146 }
gakuburu 0:728c138f0486 147 else
gakuburu 0:728c138f0486 148 {
gakuburu 0:728c138f0486 149 status = false;
gakuburu 0:728c138f0486 150 }
gakuburu 0:728c138f0486 151 }
gakuburu 0:728c138f0486 152 else
gakuburu 0:728c138f0486 153 {
gakuburu 0:728c138f0486 154 status = false;
gakuburu 0:728c138f0486 155 }
gakuburu 0:728c138f0486 156
gakuburu 0:728c138f0486 157 return status;
gakuburu 0:728c138f0486 158 }
gakuburu 0:728c138f0486 159
gakuburu 0:728c138f0486 160 int LambdaController::get_raw_axis(AxisId id)
gakuburu 0:728c138f0486 161 {
gakuburu 0:728c138f0486 162 uint8_t *data;
gakuburu 0:728c138f0486 163 int value;
gakuburu 0:728c138f0486 164
gakuburu 0:728c138f0486 165 if((int)id < 4 && (int)id >= 0)
gakuburu 0:728c138f0486 166 {
gakuburu 0:728c138f0486 167 data = &received_data[(int) id * 2 + 3];
gakuburu 0:728c138f0486 168 value = (data[0] << 8) | (data[1]);
gakuburu 0:728c138f0486 169 }
gakuburu 0:728c138f0486 170 else
gakuburu 0:728c138f0486 171 {
gakuburu 0:728c138f0486 172 value = -1;
gakuburu 0:728c138f0486 173 }
gakuburu 0:728c138f0486 174
gakuburu 0:728c138f0486 175 return value;
gakuburu 0:728c138f0486 176 }
gakuburu 0:728c138f0486 177
gakuburu 0:728c138f0486 178 double LambdaController::get_axis(AxisId id)
gakuburu 0:728c138f0486 179 {
gakuburu 0:728c138f0486 180 double value = 0;
gakuburu 0:728c138f0486 181 int raw;
gakuburu 0:728c138f0486 182 raw = get_raw_axis(id);
gakuburu 0:728c138f0486 183
gakuburu 0:728c138f0486 184 if(raw != -1)
gakuburu 0:728c138f0486 185 {
gakuburu 0:728c138f0486 186 value = (double) (raw - axis_center[(int) id]) / 512.0;
gakuburu 0:728c138f0486 187
gakuburu 0:728c138f0486 188 if (value > -axis_threshold && value < axis_threshold)
gakuburu 0:728c138f0486 189 {
gakuburu 0:728c138f0486 190 value = 0.0;
gakuburu 0:728c138f0486 191 }
gakuburu 0:728c138f0486 192 else if (value < -1.0)
gakuburu 0:728c138f0486 193 {
gakuburu 0:728c138f0486 194 value = -1.0;
gakuburu 0:728c138f0486 195 }
gakuburu 0:728c138f0486 196 else if (value > 1.0)
gakuburu 0:728c138f0486 197 {
gakuburu 0:728c138f0486 198 value = 1;
gakuburu 0:728c138f0486 199 }
gakuburu 0:728c138f0486 200 }
gakuburu 0:728c138f0486 201 else
gakuburu 0:728c138f0486 202 {
gakuburu 0:728c138f0486 203 value = 0.0;
gakuburu 0:728c138f0486 204 }
gakuburu 0:728c138f0486 205
gakuburu 0:728c138f0486 206 return value;
gakuburu 0:728c138f0486 207 }
gakuburu 0:728c138f0486 208
gakuburu 0:728c138f0486 209 void LambdaController::initialize_axis(void)
gakuburu 0:728c138f0486 210 {
gakuburu 0:728c138f0486 211 int i;
gakuburu 0:728c138f0486 212 for (i = 0; i < 4; i++)
gakuburu 0:728c138f0486 213 {
gakuburu 0:728c138f0486 214 initialize_axis((AxisId) i);
gakuburu 0:728c138f0486 215 }
gakuburu 0:728c138f0486 216 }
gakuburu 0:728c138f0486 217
gakuburu 0:728c138f0486 218 void LambdaController::initialize_axis(AxisId id)
gakuburu 0:728c138f0486 219 {
gakuburu 0:728c138f0486 220 int raw;
gakuburu 0:728c138f0486 221 raw = get_raw_axis(id);
gakuburu 0:728c138f0486 222
gakuburu 0:728c138f0486 223 if(raw != -1)
gakuburu 0:728c138f0486 224 {
gakuburu 0:728c138f0486 225 axis_center[(int) id] = raw;
gakuburu 0:728c138f0486 226 }
gakuburu 0:728c138f0486 227 }
gakuburu 0:728c138f0486 228
gakuburu 0:728c138f0486 229 LambdaController::ErrorFactor LambdaController::get_error()
gakuburu 0:728c138f0486 230 {
gakuburu 0:728c138f0486 231 if (error_timer >= timeout_time)
gakuburu 0:728c138f0486 232 {
gakuburu 0:728c138f0486 233 error_id = TimeoutError;
gakuburu 0:728c138f0486 234 }
gakuburu 0:728c138f0486 235
gakuburu 0:728c138f0486 236 return error_id;
gakuburu 0:728c138f0486 237 }