SCRIBE
The servo version of SCRIBE
Dependencies: BLE_nRF8001 BNO055 HC_SR04_Ultrasonic_Library mbed-rtos mbed
Fork of
SCRIBE_stepper
by 
SCRIBE
main.cpp@14:82248fb06e53, 2016-05-12 (annotated)
- Committer:
- manz
- Date:
- Thu May 12 06:17:49 2016 +0000
- Revision:
- 14:82248fb06e53
- Parent:
- 13:d49cb8b52a1e
latest version;
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| manz | 9:946d400b2f13 | 1 | // main of SCRIBE servo |
| manz | 0:83acd45a2405 | 2 | |
| manz | 0:83acd45a2405 | 3 | // Import libraries |
| manz | 0:83acd45a2405 | 4 | #include "Arduino.h" |
| manz | 0:83acd45a2405 | 5 | #include "BLEPeripheral.h" |
| manz | 0:83acd45a2405 | 6 | #include "mbed.h" |
| manz | 5:1da4d4050306 | 7 | #include "rtos.h" |
| manz | 5:1da4d4050306 | 8 | #include "localization.h" |
| manz | 7:1bb3b5b66fe8 | 9 | #include "servo.h" |
| manz | 0:83acd45a2405 | 10 | |
| manz | 0:83acd45a2405 | 11 | Serial serial(USBTX, USBRX); |
| manz | 7:1bb3b5b66fe8 | 12 | |
| manz | 9:946d400b2f13 | 13 | //Analog In for ADC A0 -> p15 |
| manz | 9:946d400b2f13 | 14 | AnalogIn ain1(A0); |
| manz | 9:946d400b2f13 | 15 | AnalogIn ain2(A1); |
| manz | 7:1bb3b5b66fe8 | 16 | |
| manz | 7:1bb3b5b66fe8 | 17 | //Digital LED |
| manz | 7:1bb3b5b66fe8 | 18 | DigitalOut led1(LED1); |
| manz | 7:1bb3b5b66fe8 | 19 | DigitalOut led2(LED2); |
| manz | 7:1bb3b5b66fe8 | 20 | DigitalOut led3(LED3); |
| manz | 7:1bb3b5b66fe8 | 21 | DigitalOut led4(LED4); |
| manz | 7:1bb3b5b66fe8 | 22 | |
| manz | 7:1bb3b5b66fe8 | 23 | |
| manz | 0:83acd45a2405 | 24 | SPI spi(p11, p12, p13); |
| manz | 0:83acd45a2405 | 25 | DigitalInOut BLE_RDY(p21); |
| manz | 0:83acd45a2405 | 26 | DigitalInOut BLE_REQ(p22); |
| manz | 0:83acd45a2405 | 27 | DigitalInOut BLE_RESET(p23); |
| manz | 14:82248fb06e53 | 28 | |
| manz | 14:82248fb06e53 | 29 | |
| manz | 14:82248fb06e53 | 30 | // pins for IR localization |
| manz | 14:82248fb06e53 | 31 | I2C i2c(p9, p10); // sda, scl |
| manz | 14:82248fb06e53 | 32 | const int addr = 0xB0; // define the I2C Address of camera |
| manz | 14:82248fb06e53 | 33 | int ir_finish = 0; |
| manz | 14:82248fb06e53 | 34 | |
| manz | 14:82248fb06e53 | 35 | void back_to_zero(); |
| manz | 0:83acd45a2405 | 36 | |
| manz | 0:83acd45a2405 | 37 | unsigned char txbuf[16] = {0}; |
| manz | 0:83acd45a2405 | 38 | unsigned char txlen = 0; |
| manz | 0:83acd45a2405 | 39 | |
| manz | 0:83acd45a2405 | 40 | // create peripheral |
| manz | 0:83acd45a2405 | 41 | BLEPeripheral blePeripheral = BLEPeripheral(&BLE_REQ, &BLE_RDY, &BLE_RESET); |
| manz | 0:83acd45a2405 | 42 | |
| manz | 0:83acd45a2405 | 43 | // create service w/ uuid |
| manz | 0:83acd45a2405 | 44 | BLEService uartService = BLEService("713d0000503e4c75ba943148f18d941e"); |
| manz | 0:83acd45a2405 | 45 | |
| manz | 0:83acd45a2405 | 46 | // create characteristics |
| manz | 0:83acd45a2405 | 47 | BLECharacteristic txCharacteristic = BLECharacteristic("713d0002503e4c75ba943148f18d941e", BLENotify, 20); |
| manz | 0:83acd45a2405 | 48 | BLECharacteristic rxCharacteristic = BLECharacteristic("713d0003503e4c75ba943148f18d941e", BLEWriteWithoutResponse, 20); |
| manz | 0:83acd45a2405 | 49 | |
| manz | 0:83acd45a2405 | 50 | unsigned int interval = 0; |
| manz | 0:83acd45a2405 | 51 | unsigned char count_on = 0; |
| manz | 0:83acd45a2405 | 52 | |
| manz | 5:1da4d4050306 | 53 | int which_thread = 0; |
| manz | 5:1da4d4050306 | 54 | |
| manz | 0:83acd45a2405 | 55 | // array to save the values of the user |
| manz | 7:1bb3b5b66fe8 | 56 | const int size = 500; |
| manz | 0:83acd45a2405 | 57 | int path_x [size]; |
| manz | 0:83acd45a2405 | 58 | int path_y [size]; |
| manz | 5:1da4d4050306 | 59 | int path_p [size]; |
| manz | 0:83acd45a2405 | 60 | int counter = 0; |
| manz | 0:83acd45a2405 | 61 | int start = 0; |
| manz | 0:83acd45a2405 | 62 | |
| manz | 7:1bb3b5b66fe8 | 63 | |
| manz | 3:63aef644e6d2 | 64 | const int shape_size = 10; |
| manz | 7:1bb3b5b66fe8 | 65 | double shape_x [shape_size]; |
| manz | 7:1bb3b5b66fe8 | 66 | double shape_y [shape_size]; |
| manz | 3:63aef644e6d2 | 67 | |
| manz | 5:1da4d4050306 | 68 | int mode = -1; //default mode is idle |
| manz | 5:1da4d4050306 | 69 | int pmode = 1; //default pen is down |
| manz | 3:63aef644e6d2 | 70 | |
| manz | 0:83acd45a2405 | 71 | //coordinates of the pen and the centre of SCRIBE at beginning |
| manz | 0:83acd45a2405 | 72 | double x_pen = 0; |
| manz | 0:83acd45a2405 | 73 | double y_pen = 0; |
| manz | 0:83acd45a2405 | 74 | double x_cent = 0; |
| manz | 0:83acd45a2405 | 75 | double y_cent = -1; |
| manz | 14:82248fb06e53 | 76 | double x_taro = 0; |
| manz | 14:82248fb06e53 | 77 | double y_taro = 0; |
| manz | 0:83acd45a2405 | 78 | |
| manz | 0:83acd45a2405 | 79 | //speed of SCRIBE |
| manz | 0:83acd45a2405 | 80 | float speed = 0.3; |
| manz | 0:83acd45a2405 | 81 | float t_factor = 0.15; |
| manz | 0:83acd45a2405 | 82 | |
| manz | 0:83acd45a2405 | 83 | |
| manz | 0:83acd45a2405 | 84 | Semaphore one_slot(1); |
| manz | 0:83acd45a2405 | 85 | |
| manz | 3:63aef644e6d2 | 86 | localization L; |
| manz | 0:83acd45a2405 | 87 | |
| manz | 14:82248fb06e53 | 88 | osThreadId bluetooth_id, mover_id,ir_id; |
| manz | 5:1da4d4050306 | 89 | |
| manz | 0:83acd45a2405 | 90 | void bluetooth_thread(void const *args){ |
| manz | 5:1da4d4050306 | 91 | bluetooth_id = Thread::gettid(); |
| manz | 5:1da4d4050306 | 92 | |
| manz | 0:83acd45a2405 | 93 | serial.printf("Serial begin!\r\n"); |
| manz | 5:1da4d4050306 | 94 | |
| manz | 5:1da4d4050306 | 95 | FILE *fp = fopen("/local/trial.m", "w"); |
| manz | 5:1da4d4050306 | 96 | if (fp == NULL) { |
| manz | 5:1da4d4050306 | 97 | serial.printf("ERROR: File open!\r\n"); |
| manz | 5:1da4d4050306 | 98 | } |
| manz | 5:1da4d4050306 | 99 | |
| manz | 0:83acd45a2405 | 100 | int x,y,x_dir,y_dir; |
| manz | 7:1bb3b5b66fe8 | 101 | int stroke = 0; |
| manz | 0:83acd45a2405 | 102 | |
| manz | 0:83acd45a2405 | 103 | /*----- BLE Utility ---------------------------------------------*/ |
| manz | 0:83acd45a2405 | 104 | // set advertised local name and service UUID |
| manz | 0:83acd45a2405 | 105 | blePeripheral.setLocalName("BLE Shield"); |
| manz | 0:83acd45a2405 | 106 | |
| manz | 0:83acd45a2405 | 107 | blePeripheral.setAdvertisedServiceUuid(uartService.uuid()); |
| manz | 0:83acd45a2405 | 108 | |
| manz | 0:83acd45a2405 | 109 | // add service and characteristic |
| manz | 0:83acd45a2405 | 110 | blePeripheral.addAttribute(uartService); |
| manz | 0:83acd45a2405 | 111 | blePeripheral.addAttribute(rxCharacteristic); |
| manz | 0:83acd45a2405 | 112 | blePeripheral.addAttribute(txCharacteristic); |
| manz | 0:83acd45a2405 | 113 | |
| manz | 0:83acd45a2405 | 114 | // begin initialization |
| manz | 0:83acd45a2405 | 115 | blePeripheral.begin(); |
| manz | 0:83acd45a2405 | 116 | /*---------------------------------------------------------------*/ |
| manz | 0:83acd45a2405 | 117 | |
| manz | 0:83acd45a2405 | 118 | serial.printf("BLE UART Peripheral begin!\r\n"); |
| manz | 0:83acd45a2405 | 119 | |
| manz | 0:83acd45a2405 | 120 | while(1) |
| manz | 0:83acd45a2405 | 121 | { |
| manz | 0:83acd45a2405 | 122 | BLECentral central = blePeripheral.central(); |
| manz | 7:1bb3b5b66fe8 | 123 | |
| manz | 0:83acd45a2405 | 124 | if (central) |
| manz | 0:83acd45a2405 | 125 | { |
| manz | 0:83acd45a2405 | 126 | // central connected to peripheral |
| manz | 0:83acd45a2405 | 127 | serial.printf("Connected to central\r\n"); |
| manz | 0:83acd45a2405 | 128 | while (central.connected()) |
| manz | 5:1da4d4050306 | 129 | { |
| manz | 7:1bb3b5b66fe8 | 130 | //serial.printf("*** Connected to bluetooth \r\n"); |
| manz | 7:1bb3b5b66fe8 | 131 | Thread::wait(1); |
| manz | 0:83acd45a2405 | 132 | // central still connected to peripheral |
| manz | 0:83acd45a2405 | 133 | if (rxCharacteristic.written()) |
| manz | 0:83acd45a2405 | 134 | { |
| manz | 0:83acd45a2405 | 135 | unsigned char rxlen = rxCharacteristic.valueLength(); |
| manz | 0:83acd45a2405 | 136 | const unsigned char *val = rxCharacteristic.value(); |
| manz | 5:1da4d4050306 | 137 | /*serial.printf("didCharacteristicWritten, Length: %d\r\n", rxlen); |
| manz | 3:63aef644e6d2 | 138 | |
| manz | 0:83acd45a2405 | 139 | unsigned char i = 0; |
| manz | 0:83acd45a2405 | 140 | while(i<rxlen) |
| manz | 0:83acd45a2405 | 141 | { |
| manz | 0:83acd45a2405 | 142 | serial.printf("%d, ",val[i++]); |
| manz | 0:83acd45a2405 | 143 | } |
| manz | 3:63aef644e6d2 | 144 | |
| manz | 5:1da4d4050306 | 145 | serial.printf("\r\n");*/ |
| manz | 3:63aef644e6d2 | 146 | //determine mode of signal |
| manz | 3:63aef644e6d2 | 147 | if(rxlen == 1){ |
| manz | 5:1da4d4050306 | 148 | // inputs blocked until shape finished |
| manz | 3:63aef644e6d2 | 149 | if (mode != 13){ |
| manz | 3:63aef644e6d2 | 150 | mode = (int) val[0]; |
| manz | 14:82248fb06e53 | 151 | serial.printf("Mode 1-val: %i\r\n",mode); |
| manz | 5:1da4d4050306 | 152 | if(mode != 4 && mode !=6){ |
| manz | 7:1bb3b5b66fe8 | 153 | osSignalClear(bluetooth_id,0x1); |
| manz | 5:1da4d4050306 | 154 | which_thread = 1; |
| manz | 5:1da4d4050306 | 155 | } |
| manz | 5:1da4d4050306 | 156 | } |
| manz | 0:83acd45a2405 | 157 | } |
| manz | 5:1da4d4050306 | 158 | if (mode == 6){ |
| manz | 7:1bb3b5b66fe8 | 159 | stroke = 0; |
| manz | 5:1da4d4050306 | 160 | serial.printf("Storing drawn coordinates\r\n"); |
| manz | 5:1da4d4050306 | 161 | serial.printf("x_coord = ["); |
| manz | 5:1da4d4050306 | 162 | for (int i = 0; i < size; i++) { |
| manz | 5:1da4d4050306 | 163 | serial.printf("%i ", path_x[i]); |
| manz | 5:1da4d4050306 | 164 | } |
| manz | 5:1da4d4050306 | 165 | serial.printf("];\n"); |
| manz | 5:1da4d4050306 | 166 | serial.printf("y_coord = ["); |
| manz | 5:1da4d4050306 | 167 | for (int i = 0; i < size; i++) { |
| manz | 5:1da4d4050306 | 168 | serial.printf("%i ", path_y[i]); |
| manz | 5:1da4d4050306 | 169 | } |
| manz | 5:1da4d4050306 | 170 | serial.printf("];\n"); |
| manz | 5:1da4d4050306 | 171 | } |
| manz | 5:1da4d4050306 | 172 | // stroke of pen finished |
| manz | 5:1da4d4050306 | 173 | if (mode == 7){ |
| manz | 7:1bb3b5b66fe8 | 174 | stroke = 1; |
| manz | 5:1da4d4050306 | 175 | serial.printf("Other threads can proceed \r\n"); |
| manz | 5:1da4d4050306 | 176 | osSignalClear(bluetooth_id,0x1); |
| manz | 5:1da4d4050306 | 177 | which_thread = 1; |
| manz | 5:1da4d4050306 | 178 | } |
| manz | 5:1da4d4050306 | 179 | if(rxlen == 5){ |
| manz | 5:1da4d4050306 | 180 | // in coordinates mode - accept negative coordinates |
| manz | 7:1bb3b5b66fe8 | 181 | if(mode != 4 && stroke == 0){ |
| manz | 3:63aef644e6d2 | 182 | mode = 9; |
| manz | 5:1da4d4050306 | 183 | } |
| manz | 5:1da4d4050306 | 184 | // in draw mode - convert coordinates |
| manz | 5:1da4d4050306 | 185 | x = (int) val[0]*256 + val[1]; |
| manz | 5:1da4d4050306 | 186 | y = (int) val[2]*256 + val[3]; |
| manz | 3:63aef644e6d2 | 187 | |
| manz | 3:63aef644e6d2 | 188 | // putting values into array |
| manz | 5:1da4d4050306 | 189 | //serial.printf("try to put coordinates \r\n"); |
| manz | 3:63aef644e6d2 | 190 | one_slot.wait(); |
| manz | 3:63aef644e6d2 | 191 | if (counter == size){ |
| manz | 5:1da4d4050306 | 192 | serial.printf("Overwriting values \r\n"); |
| manz | 3:63aef644e6d2 | 193 | counter = 0; |
| manz | 3:63aef644e6d2 | 194 | } |
| manz | 3:63aef644e6d2 | 195 | path_x[counter] = x; |
| manz | 3:63aef644e6d2 | 196 | path_y[counter] = y; |
| manz | 5:1da4d4050306 | 197 | |
| manz | 5:1da4d4050306 | 198 | //check if pen needs to be up or down |
| manz | 7:1bb3b5b66fe8 | 199 | path_p[counter] = (int) val[4]; |
| manz | 7:1bb3b5b66fe8 | 200 | serial.printf("Pen mode: %i \r\n",path_p[counter]); |
| manz | 5:1da4d4050306 | 201 | |
| manz | 3:63aef644e6d2 | 202 | counter++; |
| manz | 3:63aef644e6d2 | 203 | one_slot.release(); |
| manz | 5:1da4d4050306 | 204 | if(mode == 9){ |
| manz | 7:1bb3b5b66fe8 | 205 | serial.printf("coordinate mode"); |
| manz | 7:1bb3b5b66fe8 | 206 | osSignalClear(bluetooth_id,0x1); |
| manz | 5:1da4d4050306 | 207 | which_thread = 1; |
| manz | 5:1da4d4050306 | 208 | } |
| manz | 5:1da4d4050306 | 209 | //serial.printf("put coordinates \r\n"); |
| manz | 0:83acd45a2405 | 210 | } |
| manz | 0:83acd45a2405 | 211 | } |
| manz | 0:83acd45a2405 | 212 | |
| manz | 0:83acd45a2405 | 213 | if(serial.readable()) |
| manz | 0:83acd45a2405 | 214 | { |
| manz | 0:83acd45a2405 | 215 | if(!count_on) |
| manz | 0:83acd45a2405 | 216 | { |
| manz | 0:83acd45a2405 | 217 | count_on = 1; |
| manz | 0:83acd45a2405 | 218 | } |
| manz | 0:83acd45a2405 | 219 | interval = 0; |
| manz | 0:83acd45a2405 | 220 | txbuf[txlen] = serial.getc(); |
| manz | 0:83acd45a2405 | 221 | txlen++; |
| manz | 0:83acd45a2405 | 222 | } |
| manz | 0:83acd45a2405 | 223 | |
| manz | 0:83acd45a2405 | 224 | if(count_on) // Count the interval after receiving a new char from terminate |
| manz | 0:83acd45a2405 | 225 | { |
| manz | 0:83acd45a2405 | 226 | interval++; |
| manz | 0:83acd45a2405 | 227 | } |
| manz | 0:83acd45a2405 | 228 | |
| manz | 0:83acd45a2405 | 229 | if(interval == 10) // If there is no char available last the interval, send the received chars to central. |
| manz | 0:83acd45a2405 | 230 | { |
| manz | 0:83acd45a2405 | 231 | interval = 0; |
| manz | 0:83acd45a2405 | 232 | count_on = 0; |
| manz | 5:1da4d4050306 | 233 | //serial.printf("Received from terminal: %d bytes\r\n", txlen); |
| manz | 0:83acd45a2405 | 234 | txCharacteristic.setValue((const unsigned char *)txbuf, txlen); |
| manz | 0:83acd45a2405 | 235 | txlen = 0; |
| manz | 0:83acd45a2405 | 236 | } |
| manz | 0:83acd45a2405 | 237 | } |
| manz | 5:1da4d4050306 | 238 | |
| manz | 0:83acd45a2405 | 239 | // central disconnected |
| manz | 0:83acd45a2405 | 240 | serial.printf("Disconnected from central\r\n"); |
| manz | 0:83acd45a2405 | 241 | } |
| manz | 0:83acd45a2405 | 242 | } |
| manz | 0:83acd45a2405 | 243 | } |
| manz | 0:83acd45a2405 | 244 | |
| manz | 14:82248fb06e53 | 245 | void i2c_write2(int addr, char a, char b) |
| manz | 14:82248fb06e53 | 246 | { |
| manz | 14:82248fb06e53 | 247 | char cmd[2]; |
| manz | 14:82248fb06e53 | 248 | |
| manz | 14:82248fb06e53 | 249 | cmd[0] = a; |
| manz | 14:82248fb06e53 | 250 | cmd[1] = b; |
| manz | 14:82248fb06e53 | 251 | i2c.write(addr, cmd, 2); |
| manz | 14:82248fb06e53 | 252 | wait(0.07); // delay 70ms |
| manz | 14:82248fb06e53 | 253 | } |
| manz | 14:82248fb06e53 | 254 | |
| manz | 14:82248fb06e53 | 255 | void clock_init() |
| manz | 14:82248fb06e53 | 256 | { |
| manz | 14:82248fb06e53 | 257 | // set up ~20-25MHz clock on p21 |
| manz | 14:82248fb06e53 | 258 | LPC_PWM1->TCR = (1 << 1); // Reset counter, disable PWM |
| manz | 14:82248fb06e53 | 259 | LPC_SC->PCLKSEL0 &= ~(0x3 << 12); |
| manz | 14:82248fb06e53 | 260 | LPC_SC->PCLKSEL0 |= (1 << 12); // Set peripheral clock divider to /1, i.e. system clock |
| manz | 14:82248fb06e53 | 261 | LPC_PWM1->MR0 = 4; // Match Register 0 is shared period counter for all PWM1 |
| manz | 14:82248fb06e53 | 262 | LPC_PWM1->MR6 = 2; // Pin 21 is PWM output 6, so Match Register 6 |
| manz | 14:82248fb06e53 | 263 | LPC_PWM1->LER |= 1; // Start updating at next period start |
| manz | 14:82248fb06e53 | 264 | LPC_PWM1->TCR = (1 << 0) || (1 << 3); // Enable counter and PWM |
| manz | 14:82248fb06e53 | 265 | } |
| manz | 14:82248fb06e53 | 266 | |
| manz | 14:82248fb06e53 | 267 | void cam_init() |
| manz | 14:82248fb06e53 | 268 | { |
| manz | 14:82248fb06e53 | 269 | // Init IR Camera sensor |
| manz | 14:82248fb06e53 | 270 | i2c_write2(addr, 0x30, 0x01); |
| manz | 14:82248fb06e53 | 271 | i2c_write2(addr, 0x30, 0x08); |
| manz | 14:82248fb06e53 | 272 | i2c_write2(addr, 0x06, 0x90); |
| manz | 14:82248fb06e53 | 273 | i2c_write2(addr, 0x08, 0xC0); |
| manz | 14:82248fb06e53 | 274 | i2c_write2(addr, 0x1A, 0x40); |
| manz | 14:82248fb06e53 | 275 | i2c_write2(addr, 0x33, 0x33); |
| manz | 14:82248fb06e53 | 276 | wait(0.1); |
| manz | 14:82248fb06e53 | 277 | } |
| manz | 14:82248fb06e53 | 278 | |
| manz | 14:82248fb06e53 | 279 | void ir_localization(void const *args) { |
| manz | 14:82248fb06e53 | 280 | ir_id = Thread::gettid(); |
| manz | 14:82248fb06e53 | 281 | char cmd[8]; |
| manz | 14:82248fb06e53 | 282 | char buf[16]; |
| manz | 14:82248fb06e53 | 283 | int Ix1,Iy1,Ix2,Iy2; |
| manz | 14:82248fb06e53 | 284 | int Ix3,Iy3,Ix4,Iy4; |
| manz | 14:82248fb06e53 | 285 | double angle1,angle2,angle3,angle4; |
| manz | 14:82248fb06e53 | 286 | double angle_init, sum_x; |
| manz | 14:82248fb06e53 | 287 | int s; |
| manz | 14:82248fb06e53 | 288 | |
| manz | 14:82248fb06e53 | 289 | // variables for localization |
| manz | 14:82248fb06e53 | 290 | int m_count = 0; |
| manz | 14:82248fb06e53 | 291 | double alpha1,alpha2,phi1,phi2,phiIMU1,phiIMU2,beta; |
| manz | 14:82248fb06e53 | 292 | double x,y,z; |
| manz | 14:82248fb06e53 | 293 | |
| manz | 14:82248fb06e53 | 294 | // height is given |
| manz | 14:82248fb06e53 | 295 | double h = 97; |
| manz | 14:82248fb06e53 | 296 | |
| manz | 14:82248fb06e53 | 297 | //clock_init(); |
| manz | 14:82248fb06e53 | 298 | |
| manz | 14:82248fb06e53 | 299 | // PC serial output |
| manz | 14:82248fb06e53 | 300 | serial.printf("Initializing camera..."); |
| manz | 14:82248fb06e53 | 301 | |
| manz | 14:82248fb06e53 | 302 | cam_init(); |
| manz | 14:82248fb06e53 | 303 | |
| manz | 14:82248fb06e53 | 304 | serial.printf("complete\n"); |
| manz | 14:82248fb06e53 | 305 | |
| manz | 14:82248fb06e53 | 306 | float testAngle = L.getAngle(); |
| manz | 14:82248fb06e53 | 307 | angle_init = L.getAngle(); |
| manz | 14:82248fb06e53 | 308 | |
| manz | 14:82248fb06e53 | 309 | serial.printf("Initial IMU-Angle is: %f\r\n",angle_init); |
| manz | 14:82248fb06e53 | 310 | servo_slowleft(); |
| manz | 14:82248fb06e53 | 311 | // read I2C stuff |
| manz | 14:82248fb06e53 | 312 | while(m_count !=2){ |
| manz | 14:82248fb06e53 | 313 | // IR Sensor read |
| manz | 14:82248fb06e53 | 314 | cmd[0] = 0x36; |
| manz | 14:82248fb06e53 | 315 | i2c.write(addr, cmd, 1); |
| manz | 14:82248fb06e53 | 316 | i2c.read(addr, buf, 16); // read the 16-byte result |
| manz | 14:82248fb06e53 | 317 | |
| manz | 14:82248fb06e53 | 318 | Ix1 = buf[1]; |
| manz | 14:82248fb06e53 | 319 | Iy1 = buf[2]; |
| manz | 14:82248fb06e53 | 320 | s = buf[3]; |
| manz | 14:82248fb06e53 | 321 | Ix1 += (s & 0x30) <<4; |
| manz | 14:82248fb06e53 | 322 | Iy1 += (s & 0xC0) <<2; |
| manz | 14:82248fb06e53 | 323 | |
| manz | 14:82248fb06e53 | 324 | Ix2 = buf[4]; |
| manz | 14:82248fb06e53 | 325 | Iy2 = buf[5]; |
| manz | 14:82248fb06e53 | 326 | s = buf[6]; |
| manz | 14:82248fb06e53 | 327 | Ix2 += (s & 0x30) <<4; |
| manz | 14:82248fb06e53 | 328 | Iy2 += (s & 0xC0) <<2; |
| manz | 14:82248fb06e53 | 329 | |
| manz | 14:82248fb06e53 | 330 | Ix3 = buf[7]; |
| manz | 14:82248fb06e53 | 331 | Iy3 = buf[8]; |
| manz | 14:82248fb06e53 | 332 | s = buf[9]; |
| manz | 14:82248fb06e53 | 333 | Ix3 += (s & 0x30) <<4; |
| manz | 14:82248fb06e53 | 334 | Iy3 += (s & 0xC0) <<2; |
| manz | 14:82248fb06e53 | 335 | |
| manz | 14:82248fb06e53 | 336 | Ix4 = buf[10]; |
| manz | 14:82248fb06e53 | 337 | Iy4 = buf[11]; |
| manz | 14:82248fb06e53 | 338 | s = buf[12]; |
| manz | 14:82248fb06e53 | 339 | Ix4 += (s & 0x30) <<4; |
| manz | 14:82248fb06e53 | 340 | Iy4 += (s & 0xC0) <<2; |
| manz | 14:82248fb06e53 | 341 | |
| manz | 14:82248fb06e53 | 342 | angle1 = 45*((double)Ix1 / 1023 - 0.5); |
| manz | 14:82248fb06e53 | 343 | angle2 = 45*((double)Ix2 / 1023 - 0.5); |
| manz | 14:82248fb06e53 | 344 | angle3 = 45*(double)Ix3 / 1023; |
| manz | 14:82248fb06e53 | 345 | angle4 = 45*(double)Ix4 / 1023; |
| manz | 14:82248fb06e53 | 346 | |
| manz | 14:82248fb06e53 | 347 | // print the coordinate data |
| manz | 14:82248fb06e53 | 348 | serial.printf("Ix1: %4d, Iy1: %4d\n", Ix1, Iy1 ); |
| manz | 14:82248fb06e53 | 349 | //serial.printf("Ix2: %4d, Iy2: %4d\n", Ix2, Iy2 ); |
| manz | 14:82248fb06e53 | 350 | //serial.printf("Current IMU-Angle is: %f\r\n",L.getAngle()); |
| manz | 14:82248fb06e53 | 351 | //print when more than one LED present |
| manz | 14:82248fb06e53 | 352 | |
| manz | 14:82248fb06e53 | 353 | if(Ix1 < 1023){ |
| manz | 14:82248fb06e53 | 354 | //serial.printf("%d, %d: %f; %d, %d : %f\r\n", Ix1, Iy1, angle1, Ix2, Iy2, angle2); |
| manz | 14:82248fb06e53 | 355 | /* if(angle1 <= -2){ |
| manz | 14:82248fb06e53 | 356 | //serial.printf("Turn left! \r\n"); |
| manz | 14:82248fb06e53 | 357 | } |
| manz | 14:82248fb06e53 | 358 | else if (angle1 >= 2 && (abs(360 - L.getAngle()) > 90)){ |
| manz | 14:82248fb06e53 | 359 | serial.printf("Turn right! \r\n"); |
| manz | 14:82248fb06e53 | 360 | }*/ |
| manz | 14:82248fb06e53 | 361 | if(abs(angle1) < 2) { |
| manz | 14:82248fb06e53 | 362 | if(m_count == 0 && abs(360 - L.getAngle()) < 90){ |
| manz | 14:82248fb06e53 | 363 | sum_x = 0; |
| manz | 14:82248fb06e53 | 364 | // get ten pictures |
| manz | 14:82248fb06e53 | 365 | for(int i = 0; i < 10; i++){ |
| manz | 14:82248fb06e53 | 366 | cmd[0] = 0x36; |
| manz | 14:82248fb06e53 | 367 | i2c.write(addr, cmd, 1); |
| manz | 14:82248fb06e53 | 368 | i2c.read(addr, buf, 16); |
| manz | 14:82248fb06e53 | 369 | |
| manz | 14:82248fb06e53 | 370 | Ix1 = buf[1]; |
| manz | 14:82248fb06e53 | 371 | Iy1 = buf[2]; |
| manz | 14:82248fb06e53 | 372 | s = buf[3]; |
| manz | 14:82248fb06e53 | 373 | Ix1 += (s & 0x30) <<4; |
| manz | 14:82248fb06e53 | 374 | Iy1 += (s & 0xC0) <<2; |
| manz | 14:82248fb06e53 | 375 | angle1 = 45*((double)Ix1 / 1023 - 0.5); |
| manz | 14:82248fb06e53 | 376 | sum_x = sum_x + angle1; |
| manz | 14:82248fb06e53 | 377 | } |
| manz | 14:82248fb06e53 | 378 | serial.printf("*** Got average angle1: %f\r\n",sum_x/10); |
| manz | 14:82248fb06e53 | 379 | phiIMU1 = L.getAngle(); |
| manz | 14:82248fb06e53 | 380 | phi1 = 360 - (phiIMU1 + sum_x/10); |
| manz | 14:82248fb06e53 | 381 | serial.printf("*** Got phi1: %f, got phi_IMU1: %f\r\n", phi1,phiIMU1); |
| manz | 14:82248fb06e53 | 382 | m_count = 1; |
| manz | 14:82248fb06e53 | 383 | } |
| manz | 14:82248fb06e53 | 384 | else if (m_count == 1 && abs(360 - L.getAngle()) > 90){ |
| manz | 14:82248fb06e53 | 385 | servo_stop(); |
| manz | 14:82248fb06e53 | 386 | phiIMU2 = L.getAngle(); |
| manz | 14:82248fb06e53 | 387 | |
| manz | 14:82248fb06e53 | 388 | sum_x = 0; |
| manz | 14:82248fb06e53 | 389 | // get ten pictures |
| manz | 14:82248fb06e53 | 390 | for(int i = 0; i < 10; i++){ |
| manz | 14:82248fb06e53 | 391 | cmd[0] = 0x36; |
| manz | 14:82248fb06e53 | 392 | i2c.write(addr, cmd, 1); |
| manz | 14:82248fb06e53 | 393 | i2c.read(addr, buf, 16); |
| manz | 14:82248fb06e53 | 394 | |
| manz | 14:82248fb06e53 | 395 | Ix1 = buf[1]; |
| manz | 14:82248fb06e53 | 396 | Iy1 = buf[2]; |
| manz | 14:82248fb06e53 | 397 | s = buf[3]; |
| manz | 14:82248fb06e53 | 398 | Ix1 += (s & 0x30) <<4; |
| manz | 14:82248fb06e53 | 399 | Iy1 += (s & 0xC0) <<2; |
| manz | 14:82248fb06e53 | 400 | angle2 = 45*((double)Ix1 / 1023 - 0.5); |
| manz | 14:82248fb06e53 | 401 | |
| manz | 14:82248fb06e53 | 402 | sum_x = sum_x + angle2; |
| manz | 14:82248fb06e53 | 403 | } |
| manz | 14:82248fb06e53 | 404 | serial.printf("*** Got average angle2: %f\r\n",sum_x/10); |
| manz | 14:82248fb06e53 | 405 | |
| manz | 14:82248fb06e53 | 406 | phi2 = 360 - (phiIMU2 + sum_x/10); |
| manz | 14:82248fb06e53 | 407 | serial.printf("*** Got phi2: %f, got phi_IMU2: %f\r\n", phi2,phiIMU2); |
| manz | 14:82248fb06e53 | 408 | alpha1 = 90 - phi1; |
| manz | 14:82248fb06e53 | 409 | alpha2 = 90 - alpha1; |
| manz | 14:82248fb06e53 | 410 | beta = 180 - (phi2 - phi1) - alpha2; |
| manz | 14:82248fb06e53 | 411 | z = h*sin(beta/180*3.1415)/sin((phi2 - phi1)/180*3.1415); |
| manz | 14:82248fb06e53 | 412 | x = z*sin(phi1/180*3.1415); |
| manz | 14:82248fb06e53 | 413 | y = z*sin(alpha1/180*3.1415); |
| manz | 14:82248fb06e53 | 414 | |
| manz | 14:82248fb06e53 | 415 | serial.printf("Angles are: phi1 = %f, phi2 = %f:\r\n",phi1, phi2); |
| manz | 14:82248fb06e53 | 416 | serial.printf("Angles are: alpha1 = %f, alpha2 = %f:\r\n",alpha1, alpha2); |
| manz | 14:82248fb06e53 | 417 | |
| manz | 14:82248fb06e53 | 418 | //reset after print out and set coordinates |
| manz | 14:82248fb06e53 | 419 | m_count = 2; |
| manz | 14:82248fb06e53 | 420 | x_pen = rint(x/4.5); |
| manz | 14:82248fb06e53 | 421 | y_pen = rint((h-y)/4.5); |
| manz | 14:82248fb06e53 | 422 | |
| manz | 14:82248fb06e53 | 423 | //correction for translation |
| manz | 14:82248fb06e53 | 424 | x_pen = x_pen + 2; |
| manz | 14:82248fb06e53 | 425 | y_pen = y_pen - 2; |
| manz | 14:82248fb06e53 | 426 | x_cent = x_pen; |
| manz | 14:82248fb06e53 | 427 | y_cent = y_pen - 1; |
| manz | 14:82248fb06e53 | 428 | x_taro = x_pen; |
| manz | 14:82248fb06e53 | 429 | y_taro = y_pen; |
| manz | 14:82248fb06e53 | 430 | serial.printf("Current location: x = %f, y = %f \r\n",x,h-y); |
| manz | 14:82248fb06e53 | 431 | serial.printf("Current coordinates Pen: x = %f, y = %f \r\n",x_pen,y_pen); |
| manz | 14:82248fb06e53 | 432 | serial.printf("Current coordinates Center: x = %f, y = %f \r\n",x_cent,y_cent); |
| manz | 14:82248fb06e53 | 433 | } |
| manz | 14:82248fb06e53 | 434 | } |
| manz | 14:82248fb06e53 | 435 | |
| manz | 14:82248fb06e53 | 436 | } |
| manz | 14:82248fb06e53 | 437 | wait(0.2); |
| manz | 3:63aef644e6d2 | 438 | } |
| manz | 14:82248fb06e53 | 439 | |
| manz | 14:82248fb06e53 | 440 | //turn upside again |
| manz | 14:82248fb06e53 | 441 | back_to_zero(); |
| manz | 14:82248fb06e53 | 442 | ir_finish = 1; |
| manz | 3:63aef644e6d2 | 443 | } |
| manz | 3:63aef644e6d2 | 444 | |
| manz | 14:82248fb06e53 | 445 | void back_to_zero(){ |
| manz | 14:82248fb06e53 | 446 | float testAngle = L.getAngle(); |
| manz | 14:82248fb06e53 | 447 | serial.printf("Current Angle BTZ: %f",L.getAngle()); |
| manz | 14:82248fb06e53 | 448 | while(L.getAngle()> 1){ |
| manz | 14:82248fb06e53 | 449 | if(L.getAngle() > 180){ |
| manz | 14:82248fb06e53 | 450 | servo_slowright(); |
| manz | 14:82248fb06e53 | 451 | } |
| manz | 14:82248fb06e53 | 452 | else{ |
| manz | 14:82248fb06e53 | 453 | servo_slowleft(); |
| manz | 14:82248fb06e53 | 454 | } |
| manz | 14:82248fb06e53 | 455 | } |
| manz | 14:82248fb06e53 | 456 | servo_stop(); |
| manz | 14:82248fb06e53 | 457 | } |
| manz | 0:83acd45a2405 | 458 | |
| manz | 0:83acd45a2405 | 459 | void move_thread(void const *args){ |
| manz | 5:1da4d4050306 | 460 | |
| manz | 0:83acd45a2405 | 461 | int angle; |
| manz | 9:946d400b2f13 | 462 | double length = 8; |
| manz | 9:946d400b2f13 | 463 | double width = 8; |
| manz | 3:63aef644e6d2 | 464 | int radius = 5; |
| manz | 0:83acd45a2405 | 465 | |
| manz | 0:83acd45a2405 | 466 | Timer t; |
| manz | 0:83acd45a2405 | 467 | //initally put pen down |
| manz | 0:83acd45a2405 | 468 | |
| manz | 0:83acd45a2405 | 469 | float currentAngle; |
| manz | 0:83acd45a2405 | 470 | float targetAngle; |
| manz | 0:83acd45a2405 | 471 | float startAngle; |
| manz | 0:83acd45a2405 | 472 | float diffAngle; |
| manz | 0:83acd45a2405 | 473 | |
| manz | 5:1da4d4050306 | 474 | int steps; |
| manz | 5:1da4d4050306 | 475 | |
| manz | 0:83acd45a2405 | 476 | int control; |
| manz | 7:1bb3b5b66fe8 | 477 | int restore = 0; |
| manz | 3:63aef644e6d2 | 478 | int shape_count = 0; |
| manz | 5:1da4d4050306 | 479 | int p_mode; |
| manz | 5:1da4d4050306 | 480 | int first = 0; |
| manz | 3:63aef644e6d2 | 481 | |
| manz | 9:946d400b2f13 | 482 | double draw_corr = 2; |
| nibab | 8:4d7b2dbdb694 | 483 | double time_factor = 0.05; |
| manz | 0:83acd45a2405 | 484 | |
| manz | 0:83acd45a2405 | 485 | double dot,a2,b2,c2; |
| manz | 0:83acd45a2405 | 486 | double x_tar,y_tar; |
| manz | 7:1bb3b5b66fe8 | 487 | double x_pen_pr, y_pen_pr, x_cent_pr, y_cent_pr, x_taro_pr, y_taro_pr; |
| manz | 0:83acd45a2405 | 488 | double cosg,gamma,distance,wait_t; |
| manz | 0:83acd45a2405 | 489 | serial.printf("Started move thread\r\n"); |
| manz | 0:83acd45a2405 | 490 | int newval = 0; |
| manz | 9:946d400b2f13 | 491 | |
| manz | 9:946d400b2f13 | 492 | //values for encoding |
| manz | 9:946d400b2f13 | 493 | int counter_l = 0; |
| manz | 9:946d400b2f13 | 494 | int on_l; |
| manz | 9:946d400b2f13 | 495 | int counter_r = 0; |
| manz | 9:946d400b2f13 | 496 | int on_r; |
| manz | 9:946d400b2f13 | 497 | double sum_r = 0; |
| manz | 9:946d400b2f13 | 498 | double sum_l = 0; |
| manz | 9:946d400b2f13 | 499 | double value_r; |
| manz | 9:946d400b2f13 | 500 | double value_l; |
| manz | 12:01250ea24795 | 501 | float angle_init; |
| manz | 12:01250ea24795 | 502 | |
| manz | 12:01250ea24795 | 503 | float testAngle = L.getAngle(); |
| manz | 5:1da4d4050306 | 504 | |
| manz | 0:83acd45a2405 | 505 | while(1){ |
| manz | 3:63aef644e6d2 | 506 | // check what mode is present |
| manz | 5:1da4d4050306 | 507 | serial.printf("Cylce \r\n"); |
| manz | 7:1bb3b5b66fe8 | 508 | serial.printf("mode: %i \r\n",mode); |
| manz | 7:1bb3b5b66fe8 | 509 | |
| manz | 5:1da4d4050306 | 510 | Thread::signal_wait(0x1); |
| manz | 7:1bb3b5b66fe8 | 511 | |
| manz | 5:1da4d4050306 | 512 | if(mode == -1){ |
| manz | 5:1da4d4050306 | 513 | serial.printf("here ends"); |
| manz | 5:1da4d4050306 | 514 | osSignalClear(mover_id,0x1); |
| manz | 5:1da4d4050306 | 515 | which_thread = 0; |
| manz | 5:1da4d4050306 | 516 | } |
| manz | 3:63aef644e6d2 | 517 | //rectangle/square |
| manz | 5:1da4d4050306 | 518 | else if(mode == 0){ |
| manz | 3:63aef644e6d2 | 519 | serial.printf("Draw rectangle \r\n"); |
| manz | 3:63aef644e6d2 | 520 | //save old values and set initial |
| manz | 3:63aef644e6d2 | 521 | x_pen_pr = x_pen; |
| manz | 3:63aef644e6d2 | 522 | y_pen_pr = y_pen; |
| manz | 3:63aef644e6d2 | 523 | x_cent_pr = x_cent; |
| manz | 7:1bb3b5b66fe8 | 524 | y_cent_pr = y_cent; |
| manz | 7:1bb3b5b66fe8 | 525 | x_taro_pr = x_taro; |
| manz | 7:1bb3b5b66fe8 | 526 | y_taro_pr = y_taro; |
| manz | 3:63aef644e6d2 | 527 | x_pen = 0; |
| manz | 3:63aef644e6d2 | 528 | y_pen = 0; |
| manz | 3:63aef644e6d2 | 529 | x_cent = 0; |
| manz | 3:63aef644e6d2 | 530 | y_cent = -1; |
| manz | 5:1da4d4050306 | 531 | x_taro = 0; |
| manz | 5:1da4d4050306 | 532 | y_taro = 0; |
| manz | 3:63aef644e6d2 | 533 | |
| manz | 3:63aef644e6d2 | 534 | //set values |
| manz | 7:1bb3b5b66fe8 | 535 | shape_x[4] = 0; |
| manz | 7:1bb3b5b66fe8 | 536 | shape_y[4] = length; |
| manz | 7:1bb3b5b66fe8 | 537 | shape_x[3] = width; |
| manz | 3:63aef644e6d2 | 538 | shape_y[3] = length; |
| manz | 3:63aef644e6d2 | 539 | shape_x[2] = width; |
| manz | 7:1bb3b5b66fe8 | 540 | shape_y[2] = 0; |
| manz | 7:1bb3b5b66fe8 | 541 | shape_x[1] = 0; |
| manz | 3:63aef644e6d2 | 542 | shape_y[1] = 0; |
| manz | 3:63aef644e6d2 | 543 | shape_x[0] = 0; |
| manz | 7:1bb3b5b66fe8 | 544 | shape_y[0] = 0.1; |
| manz | 7:1bb3b5b66fe8 | 545 | shape_count = 5; |
| manz | 3:63aef644e6d2 | 546 | |
| manz | 3:63aef644e6d2 | 547 | mode = 13; |
| manz | 3:63aef644e6d2 | 548 | } |
| manz | 3:63aef644e6d2 | 549 | else if(mode == 1){ |
| manz | 3:63aef644e6d2 | 550 | serial.printf("Draw circle \r\n"); |
| manz | 3:63aef644e6d2 | 551 | //call circle function |
| manz | 14:82248fb06e53 | 552 | //control = draw_circle(radius); |
| manz | 3:63aef644e6d2 | 553 | |
| manz | 3:63aef644e6d2 | 554 | mode = -1; |
| manz | 3:63aef644e6d2 | 555 | } |
| manz | 3:63aef644e6d2 | 556 | else if(mode == 2){ |
| manz | 3:63aef644e6d2 | 557 | serial.printf("Draw triangle \r\n"); |
| manz | 3:63aef644e6d2 | 558 | //save old values and set initial |
| manz | 3:63aef644e6d2 | 559 | x_pen_pr = x_pen; |
| manz | 3:63aef644e6d2 | 560 | y_pen_pr = y_pen; |
| manz | 3:63aef644e6d2 | 561 | x_cent_pr = x_cent; |
| manz | 7:1bb3b5b66fe8 | 562 | y_cent_pr = y_cent; |
| manz | 7:1bb3b5b66fe8 | 563 | x_taro_pr = x_taro; |
| manz | 7:1bb3b5b66fe8 | 564 | y_taro_pr = y_taro; |
| manz | 3:63aef644e6d2 | 565 | x_pen = 0; |
| manz | 3:63aef644e6d2 | 566 | y_pen = 0; |
| manz | 3:63aef644e6d2 | 567 | x_cent = 0; |
| manz | 3:63aef644e6d2 | 568 | y_cent = -1; |
| manz | 5:1da4d4050306 | 569 | x_taro = 0; |
| manz | 5:1da4d4050306 | 570 | y_taro = 0; |
| manz | 3:63aef644e6d2 | 571 | |
| manz | 3:63aef644e6d2 | 572 | //set values |
| manz | 7:1bb3b5b66fe8 | 573 | shape_x[3] = 0; |
| manz | 7:1bb3b5b66fe8 | 574 | shape_y[3] = length; |
| manz | 7:1bb3b5b66fe8 | 575 | shape_x[2] = width; |
| manz | 7:1bb3b5b66fe8 | 576 | shape_y[2] = 0; |
| manz | 7:1bb3b5b66fe8 | 577 | shape_x[1] = 0; |
| manz | 3:63aef644e6d2 | 578 | shape_y[1] = 0; |
| manz | 3:63aef644e6d2 | 579 | shape_x[0] = 0; |
| manz | 7:1bb3b5b66fe8 | 580 | shape_y[0] = 0.1; |
| manz | 3:63aef644e6d2 | 581 | |
| manz | 7:1bb3b5b66fe8 | 582 | shape_count = 4; |
| manz | 3:63aef644e6d2 | 583 | mode = 13; |
| manz | 3:63aef644e6d2 | 584 | } |
| manz | 3:63aef644e6d2 | 585 | else if(mode == 3){ |
| manz | 3:63aef644e6d2 | 586 | serial.printf("Reset \r\n"); |
| manz | 3:63aef644e6d2 | 587 | //set initial |
| manz | 3:63aef644e6d2 | 588 | x_pen = 0; |
| manz | 3:63aef644e6d2 | 589 | y_pen = 0; |
| manz | 3:63aef644e6d2 | 590 | x_cent = 0; |
| manz | 3:63aef644e6d2 | 591 | y_cent = -1; |
| manz | 7:1bb3b5b66fe8 | 592 | x_taro = 0; |
| manz | 7:1bb3b5b66fe8 | 593 | y_taro = 0; |
| manz | 7:1bb3b5b66fe8 | 594 | |
| manz | 7:1bb3b5b66fe8 | 595 | one_slot.wait(); |
| manz | 7:1bb3b5b66fe8 | 596 | counter = 0; |
| manz | 7:1bb3b5b66fe8 | 597 | start = 0; |
| manz | 7:1bb3b5b66fe8 | 598 | one_slot.release(); |
| manz | 3:63aef644e6d2 | 599 | |
| manz | 3:63aef644e6d2 | 600 | mode = -1; |
| manz | 3:63aef644e6d2 | 601 | } |
| manz | 5:1da4d4050306 | 602 | else if (mode == 7){ |
| manz | 5:1da4d4050306 | 603 | if (first == 0){ |
| manz | 5:1da4d4050306 | 604 | serial.printf("Draw freely \r\n"); |
| manz | 14:82248fb06e53 | 605 | draw_corr = 0.08; |
| manz | 5:1da4d4050306 | 606 | } |
| manz | 5:1da4d4050306 | 607 | first = 1; |
| manz | 3:63aef644e6d2 | 608 | } |
| manz | 3:63aef644e6d2 | 609 | else if (mode == 9){ |
| manz | 3:63aef644e6d2 | 610 | serial.printf("Draw coordinates \r\n"); |
| manz | 12:01250ea24795 | 611 | draw_corr = 1; |
| manz | 3:63aef644e6d2 | 612 | mode = -1; |
| manz | 3:63aef644e6d2 | 613 | } |
| manz | 3:63aef644e6d2 | 614 | |
| manz | 3:63aef644e6d2 | 615 | // next coordinate is free drawing or coordinates |
| manz | 3:63aef644e6d2 | 616 | if(shape_count == 0){ |
| manz | 3:63aef644e6d2 | 617 | one_slot.wait(); |
| manz | 3:63aef644e6d2 | 618 | if(counter != start){ |
| manz | 3:63aef644e6d2 | 619 | if (start == size){ |
| manz | 3:63aef644e6d2 | 620 | start = 0; |
| manz | 3:63aef644e6d2 | 621 | } |
| manz | 14:82248fb06e53 | 622 | x_tar = (double) path_x[start]; |
| manz | 14:82248fb06e53 | 623 | y_tar = (double) path_y[start]; |
| manz | 14:82248fb06e53 | 624 | serial.printf("Got data: x = %f, y = %f \r\n",x_tar,y_tar); |
| manz | 14:82248fb06e53 | 625 | x_tar = x_tar*draw_corr; |
| manz | 14:82248fb06e53 | 626 | y_tar = y_tar*draw_corr; |
| manz | 14:82248fb06e53 | 627 | serial.printf("Scaled data: x = %f, y = %f \r\n",x_tar,y_tar); |
| manz | 5:1da4d4050306 | 628 | p_mode = path_p[start]; |
| manz | 3:63aef644e6d2 | 629 | start++; |
| manz | 7:1bb3b5b66fe8 | 630 | if(start == counter){ |
| manz | 7:1bb3b5b66fe8 | 631 | mode = -1; |
| manz | 7:1bb3b5b66fe8 | 632 | } |
| manz | 3:63aef644e6d2 | 633 | newval = 1; |
| manz | 0:83acd45a2405 | 634 | } |
| manz | 3:63aef644e6d2 | 635 | one_slot.release(); |
| manz | 0:83acd45a2405 | 636 | } |
| manz | 3:63aef644e6d2 | 637 | // next coordinate is shape |
| manz | 3:63aef644e6d2 | 638 | else{ |
| manz | 3:63aef644e6d2 | 639 | shape_count = shape_count - 1; |
| manz | 7:1bb3b5b66fe8 | 640 | x_tar = shape_x[shape_count]; |
| manz | 7:1bb3b5b66fe8 | 641 | y_tar = shape_y[shape_count]; |
| manz | 5:1da4d4050306 | 642 | p_mode = 1; |
| manz | 3:63aef644e6d2 | 643 | newval = 1; |
| manz | 5:1da4d4050306 | 644 | |
| manz | 5:1da4d4050306 | 645 | //last move -> unblock input |
| manz | 5:1da4d4050306 | 646 | if(shape_count == 0){ |
| manz | 7:1bb3b5b66fe8 | 647 | restore = 1; |
| manz | 5:1da4d4050306 | 648 | mode = -1; |
| manz | 5:1da4d4050306 | 649 | } |
| manz | 3:63aef644e6d2 | 650 | } |
| manz | 0:83acd45a2405 | 651 | if(newval == 1){ |
| manz | 0:83acd45a2405 | 652 | serial.printf("x-coord: %f, y-coord: %f\r\n",x_tar,y_tar); |
| manz | 0:83acd45a2405 | 653 | newval = 0; |
| manz | 7:1bb3b5b66fe8 | 654 | // same position -> do nothing |
| manz | 7:1bb3b5b66fe8 | 655 | if(x_tar == x_pen && y_tar == y_pen){ |
| manz | 7:1bb3b5b66fe8 | 656 | |
| manz | 5:1da4d4050306 | 657 | } |
| manz | 7:1bb3b5b66fe8 | 658 | else{ |
| manz | 7:1bb3b5b66fe8 | 659 | //compute angle and turn direction |
| manz | 7:1bb3b5b66fe8 | 660 | a2 = (x_pen - x_cent)*(x_pen - x_cent) + (y_pen - y_cent)*(y_pen - y_cent); |
| manz | 7:1bb3b5b66fe8 | 661 | b2 = (x_tar - x_pen)*(x_tar - x_pen) + (y_tar - y_pen)*(y_tar - y_pen); |
| manz | 7:1bb3b5b66fe8 | 662 | c2 = (x_tar - x_cent)*(x_tar - x_cent) + (y_tar - y_cent)*(y_tar - y_cent); |
| manz | 7:1bb3b5b66fe8 | 663 | cosg = (a2 + b2 - c2)/(2*sqrt(a2*b2)); |
| manz | 7:1bb3b5b66fe8 | 664 | gamma = acos(cosg)/3.14159*180; |
| manz | 7:1bb3b5b66fe8 | 665 | |
| manz | 7:1bb3b5b66fe8 | 666 | dot = (x_tar - x_cent)*(y_pen - y_cent) - (y_tar - y_cent)*(x_pen - x_cent); |
| manz | 7:1bb3b5b66fe8 | 667 | |
| manz | 7:1bb3b5b66fe8 | 668 | //serial.printf("Angle: %f \r\n",gamma); |
| manz | 7:1bb3b5b66fe8 | 669 | angle = ceil(180 - gamma); |
| manz | 7:1bb3b5b66fe8 | 670 | serial.printf("Turning angle: %i \r\n",angle); |
| manz | 7:1bb3b5b66fe8 | 671 | |
| manz | 7:1bb3b5b66fe8 | 672 | //put pen down |
| manz | 7:1bb3b5b66fe8 | 673 | if (p_mode == 1){ |
| manz | 7:1bb3b5b66fe8 | 674 | serial.printf("-- Pen Down\r\n"); |
| manz | 9:946d400b2f13 | 675 | L.servo(30); |
| manz | 0:83acd45a2405 | 676 | } |
| manz | 7:1bb3b5b66fe8 | 677 | //put pen up |
| manz | 7:1bb3b5b66fe8 | 678 | else if (p_mode == 0){ |
| manz | 9:946d400b2f13 | 679 | L.servo(45); |
| manz | 7:1bb3b5b66fe8 | 680 | serial.printf("-- Pen Up\r\n"); |
| manz | 7:1bb3b5b66fe8 | 681 | } |
| manz | 7:1bb3b5b66fe8 | 682 | |
| manz | 7:1bb3b5b66fe8 | 683 | currentAngle = L.getAngle(); |
| manz | 7:1bb3b5b66fe8 | 684 | if(dot > 0){ |
| manz | 7:1bb3b5b66fe8 | 685 | //serial.printf("Turn right \r\n"); |
| manz | 7:1bb3b5b66fe8 | 686 | targetAngle = fmod(currentAngle+angle,360); |
| manz | 7:1bb3b5b66fe8 | 687 | |
| manz | 7:1bb3b5b66fe8 | 688 | servo_right(); |
| manz | 12:01250ea24795 | 689 | while(fmod(abs(L.getAngle() - targetAngle),360)> 5); |
| manz | 7:1bb3b5b66fe8 | 690 | servo_stop(); |
| manz | 0:83acd45a2405 | 691 | } |
| manz | 7:1bb3b5b66fe8 | 692 | else if(dot < 0){ |
| manz | 7:1bb3b5b66fe8 | 693 | //serial.printf("Turn left \r\n"); |
| manz | 7:1bb3b5b66fe8 | 694 | targetAngle = fmod(currentAngle-angle,360); |
| manz | 7:1bb3b5b66fe8 | 695 | servo_left(); |
| manz | 12:01250ea24795 | 696 | while(fmod(abs(L.getAngle() - targetAngle),360)> 5); |
| manz | 7:1bb3b5b66fe8 | 697 | servo_stop(); |
| manz | 7:1bb3b5b66fe8 | 698 | } |
| manz | 7:1bb3b5b66fe8 | 699 | //compute length of path til target |
| manz | 7:1bb3b5b66fe8 | 700 | distance = sqrt((x_tar - x_pen)*(x_tar - x_pen) + (y_tar - y_pen)*(y_tar - y_pen)); |
| manz | 7:1bb3b5b66fe8 | 701 | |
| manz | 14:82248fb06e53 | 702 | //forward SCRIBE til target |
| manz | 9:946d400b2f13 | 703 | int counter_m = (int) rint(distance); |
| manz | 9:946d400b2f13 | 704 | |
| manz | 14:82248fb06e53 | 705 | // do adjustment when going down |
| manz | 14:82248fb06e53 | 706 | if (L.getAngle() > 330 || L.getAngle() < 30){ |
| manz | 14:82248fb06e53 | 707 | counter_m = counter_m + 2; |
| manz | 14:82248fb06e53 | 708 | } |
| manz | 14:82248fb06e53 | 709 | if (L.getAngle() > 130 || L.getAngle() < 230){ |
| manz | 14:82248fb06e53 | 710 | counter_m = counter_m - 2; |
| manz | 14:82248fb06e53 | 711 | } |
| manz | 14:82248fb06e53 | 712 | serial.printf("*** Distance to be taken: %d\r\n",counter_m); |
| manz | 14:82248fb06e53 | 713 | |
| manz | 9:946d400b2f13 | 714 | // find position of wheels |
| manz | 9:946d400b2f13 | 715 | for (int i = 0; i < 100; i++){ |
| manz | 9:946d400b2f13 | 716 | sum_r += ain1.read(); |
| manz | 9:946d400b2f13 | 717 | sum_l += ain2.read(); |
| manz | 9:946d400b2f13 | 718 | } |
| manz | 9:946d400b2f13 | 719 | value_r = sum_r/100; |
| manz | 9:946d400b2f13 | 720 | value_l = sum_l/100; |
| manz | 9:946d400b2f13 | 721 | |
| manz | 9:946d400b2f13 | 722 | if(value_r < 0.93){ |
| manz | 9:946d400b2f13 | 723 | on_r = 0; |
| manz | 9:946d400b2f13 | 724 | } |
| manz | 9:946d400b2f13 | 725 | else{ |
| manz | 9:946d400b2f13 | 726 | on_r = 1; |
| manz | 9:946d400b2f13 | 727 | } |
| manz | 14:82248fb06e53 | 728 | if(value_l < 0.925){ |
| manz | 9:946d400b2f13 | 729 | on_l = 0; |
| manz | 9:946d400b2f13 | 730 | } |
| manz | 9:946d400b2f13 | 731 | else{ |
| manz | 9:946d400b2f13 | 732 | on_l = 1; |
| manz | 9:946d400b2f13 | 733 | } |
| manz | 9:946d400b2f13 | 734 | serial.printf("Initially: left: %d, right: %d\r\n",on_l,on_r); |
| manz | 12:01250ea24795 | 735 | angle_init = L.getAngle(); |
| manz | 7:1bb3b5b66fe8 | 736 | servo_f(); |
| manz | 12:01250ea24795 | 737 | serial.printf("Initial Angle: %f\r\n",angle_init); |
| manz | 9:946d400b2f13 | 738 | //start decrement counter |
| manz | 9:946d400b2f13 | 739 | counter_r = counter_m; |
| manz | 9:946d400b2f13 | 740 | counter_l = counter_m; |
| manz | 9:946d400b2f13 | 741 | while (counter_m > 0){ |
| manz | 9:946d400b2f13 | 742 | sum_r = 0; |
| manz | 9:946d400b2f13 | 743 | sum_l = 0; |
| manz | 14:82248fb06e53 | 744 | for (int i = 0; i < 5; i++){ |
| manz | 9:946d400b2f13 | 745 | sum_r += ain1.read(); |
| manz | 9:946d400b2f13 | 746 | sum_l += ain2.read(); |
| manz | 9:946d400b2f13 | 747 | } |
| manz | 14:82248fb06e53 | 748 | value_r = sum_r/5; |
| manz | 14:82248fb06e53 | 749 | value_l = sum_l/5; |
| manz | 14:82248fb06e53 | 750 | printf("Left Value: %f\r\n",value_l); |
| manz | 14:82248fb06e53 | 751 | printf("Right Value: %f\r\n",value_r); |
| manz | 14:82248fb06e53 | 752 | if(value_r < 0.84 && on_r == 1){ |
| manz | 12:01250ea24795 | 753 | //printf("Value right: %f\r\n",value_r); |
| manz | 9:946d400b2f13 | 754 | on_r = 0; |
| manz | 9:946d400b2f13 | 755 | } |
| manz | 14:82248fb06e53 | 756 | else if (value_r > 0.88 && on_r == 0){ |
| manz | 12:01250ea24795 | 757 | //printf("Value right: %f\r\n",value_r); |
| manz | 9:946d400b2f13 | 758 | on_r = 1; |
| manz | 9:946d400b2f13 | 759 | counter_r--; |
| manz | 14:82248fb06e53 | 760 | printf("*** Right Counter is: %d\r\n",counter_r); |
| manz | 9:946d400b2f13 | 761 | } |
| manz | 14:82248fb06e53 | 762 | if(value_l < 0.9 && on_l == 1){ |
| manz | 12:01250ea24795 | 763 | //printf("Value left: %f\r\n",value_l); |
| manz | 9:946d400b2f13 | 764 | on_l = 0; |
| manz | 9:946d400b2f13 | 765 | } |
| manz | 14:82248fb06e53 | 766 | else if (value_l > 0.92 && on_l == 0){ |
| manz | 12:01250ea24795 | 767 | //printf("Value left: %f\r\n",value_l); |
| manz | 9:946d400b2f13 | 768 | on_l = 1; |
| manz | 9:946d400b2f13 | 769 | counter_l--; |
| manz | 14:82248fb06e53 | 770 | printf("*** Left Counter is: %d\r\n",counter_l); |
| manz | 9:946d400b2f13 | 771 | } |
| manz | 9:946d400b2f13 | 772 | |
| manz | 9:946d400b2f13 | 773 | if(counter_l >= counter_r){ |
| manz | 9:946d400b2f13 | 774 | counter_m = counter_r; |
| manz | 9:946d400b2f13 | 775 | } |
| manz | 9:946d400b2f13 | 776 | else{ |
| manz | 9:946d400b2f13 | 777 | counter_m = counter_l; |
| manz | 9:946d400b2f13 | 778 | } |
| manz | 12:01250ea24795 | 779 | //deviation left from path |
| manz | 14:82248fb06e53 | 780 | if(angle_init > 270 || angle_init < 90){ |
| manz | 14:82248fb06e53 | 781 | if((L.getAngle() - angle_init > 2 && L.getAngle() - angle_init < 10) || (angle_init > 358 && L.getAngle() < 5 && (L.getAngle() + 360 - angle_init) > 2)){ |
| manz | 14:82248fb06e53 | 782 | servo_slowleft(); |
| manz | 14:82248fb06e53 | 783 | serial.printf("Left: Angle Difference: %f\r\n",L.getAngle() - angle_init); |
| manz | 14:82248fb06e53 | 784 | serial.printf("Left: Init: %f, Current: %f\r\n",angle_init,L.getAngle()); |
| manz | 14:82248fb06e53 | 785 | while(fmod(abs(L.getAngle() - angle_init),360)> 1); |
| manz | 14:82248fb06e53 | 786 | servo_f(); |
| manz | 14:82248fb06e53 | 787 | } |
| manz | 14:82248fb06e53 | 788 | //deviation right from path |
| manz | 14:82248fb06e53 | 789 | if(angle_init - L.getAngle() > 2 || (L.getAngle() > 358 && angle_init < 5 && (angle_init + 360 - L.getAngle()) > 2)){ |
| manz | 14:82248fb06e53 | 790 | servo_slowright(); |
| manz | 14:82248fb06e53 | 791 | while(fmod(abs(L.getAngle() - angle_init),360)> 1); |
| manz | 14:82248fb06e53 | 792 | servo_f(); |
| manz | 14:82248fb06e53 | 793 | } |
| manz | 12:01250ea24795 | 794 | } |
| manz | 12:01250ea24795 | 795 | //printf("*** Distance to take is: %d\r\n",counter_m); |
| manz | 9:946d400b2f13 | 796 | } |
| manz | 9:946d400b2f13 | 797 | //servo_f(); |
| manz | 9:946d400b2f13 | 798 | //wait(distance*time_factor); |
| manz | 7:1bb3b5b66fe8 | 799 | servo_stop(); |
| manz | 7:1bb3b5b66fe8 | 800 | //serial.printf("Reached destination \r\n"); |
| manz | 7:1bb3b5b66fe8 | 801 | //update pen and center when at target |
| manz | 7:1bb3b5b66fe8 | 802 | if(restore == 1){ |
| manz | 7:1bb3b5b66fe8 | 803 | serial.printf("Restore previous positions \r\n"); |
| manz | 7:1bb3b5b66fe8 | 804 | x_pen = x_pen_pr; |
| manz | 7:1bb3b5b66fe8 | 805 | y_pen = y_pen_pr; |
| manz | 7:1bb3b5b66fe8 | 806 | x_cent = x_cent_pr; |
| manz | 7:1bb3b5b66fe8 | 807 | y_cent = y_cent_pr; |
| manz | 7:1bb3b5b66fe8 | 808 | x_taro = x_taro_pr; |
| manz | 7:1bb3b5b66fe8 | 809 | y_taro = y_taro_pr; |
| manz | 7:1bb3b5b66fe8 | 810 | restore = 0; |
| manz | 7:1bb3b5b66fe8 | 811 | } |
| manz | 7:1bb3b5b66fe8 | 812 | else{ |
| manz | 7:1bb3b5b66fe8 | 813 | x_pen = x_tar; |
| manz | 7:1bb3b5b66fe8 | 814 | y_pen = y_tar; |
| manz | 7:1bb3b5b66fe8 | 815 | x_cent = x_taro; |
| manz | 7:1bb3b5b66fe8 | 816 | y_cent = y_taro; |
| manz | 7:1bb3b5b66fe8 | 817 | x_taro = x_tar; |
| manz | 7:1bb3b5b66fe8 | 818 | y_taro = y_tar; |
| manz | 7:1bb3b5b66fe8 | 819 | } |
| manz | 7:1bb3b5b66fe8 | 820 | } |
| manz | 0:83acd45a2405 | 821 | serial.printf("Update Pen: %f, %f \r\n",x_pen,y_pen); |
| manz | 7:1bb3b5b66fe8 | 822 | serial.printf("Update Center: %f, %f \r\n",x_cent,y_cent); |
| manz | 5:1da4d4050306 | 823 | } |
| manz | 0:83acd45a2405 | 824 | } |
| manz | 0:83acd45a2405 | 825 | } |
| manz | 0:83acd45a2405 | 826 | |
| manz | 5:1da4d4050306 | 827 | |
| manz | 5:1da4d4050306 | 828 | |
| manz | 0:83acd45a2405 | 829 | int main() |
| manz | 7:1bb3b5b66fe8 | 830 | { |
| manz | 7:1bb3b5b66fe8 | 831 | L.reset(); |
| manz | 14:82248fb06e53 | 832 | servo_reset(); |
| manz | 14:82248fb06e53 | 833 | servo_stop(); |
| manz | 14:82248fb06e53 | 834 | serial.printf("Starting Calibration\r\n"); |
| manz | 14:82248fb06e53 | 835 | back_to_zero(); |
| manz | 14:82248fb06e53 | 836 | |
| manz | 14:82248fb06e53 | 837 | serial.printf("Starting IR-Localization\r\n"); |
| manz | 14:82248fb06e53 | 838 | //ir_localization(); |
| manz | 14:82248fb06e53 | 839 | Thread ir(ir_localization); |
| manz | 14:82248fb06e53 | 840 | ir_id = ir.gettid(); |
| manz | 14:82248fb06e53 | 841 | |
| manz | 14:82248fb06e53 | 842 | while(ir_finish == 0){ |
| manz | 14:82248fb06e53 | 843 | Thread::wait(10); |
| manz | 14:82248fb06e53 | 844 | } |
| manz | 7:1bb3b5b66fe8 | 845 | serial.printf("Starting the threads\r\n"); |
| manz | 0:83acd45a2405 | 846 | |
| manz | 0:83acd45a2405 | 847 | Thread bluetooth(bluetooth_thread); |
| manz | 0:83acd45a2405 | 848 | Thread move(move_thread); |
| manz | 5:1da4d4050306 | 849 | mover_id = move.gettid(); |
| manz | 5:1da4d4050306 | 850 | bluetooth_id = bluetooth.gettid(); |
| manz | 0:83acd45a2405 | 851 | |
| manz | 5:1da4d4050306 | 852 | while(1){ |
| manz | 7:1bb3b5b66fe8 | 853 | Thread::wait(3); |
| manz | 5:1da4d4050306 | 854 | if(which_thread == 0) bluetooth.signal_set(0x1); |
| manz | 5:1da4d4050306 | 855 | else move.signal_set(0x1); |
| manz | 5:1da4d4050306 | 856 | } |
| manz | 0:83acd45a2405 | 857 | } |