Port of the LPC4088 QSB EA LCD Sphere Demo to the LPC4088 DM using the DMSupport lib. Dropping the QSB EALib
Dependencies: DMBasicGUI DMSupport
Fork of lpc4088_displaymodule_hello_world by
Just a quick hack to get something good looking on the LPC4088DM
/media/uploads/tecnosys/lpc4088dm.jpg
SphereDemo.cpp@5:d2f1c9185c5b, 2015-04-29 (annotated)
- Committer:
- tecnosys
- Date:
- Wed Apr 29 05:34:03 2015 +0000
- Revision:
- 5:d2f1c9185c5b
initial port of EA LCD Demo to DMSupport libs
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tecnosys | 5:d2f1c9185c5b | 1 | /****************************************************************************** |
tecnosys | 5:d2f1c9185c5b | 2 | * Includes |
tecnosys | 5:d2f1c9185c5b | 3 | *****************************************************************************/ |
tecnosys | 5:d2f1c9185c5b | 4 | |
tecnosys | 5:d2f1c9185c5b | 5 | #include "mbed.h" |
tecnosys | 5:d2f1c9185c5b | 6 | #include "DMBoard.h" |
tecnosys | 5:d2f1c9185c5b | 7 | //#include "LcdController.h" |
tecnosys | 5:d2f1c9185c5b | 8 | #//include "EaLcdBoard.h" |
tecnosys | 5:d2f1c9185c5b | 9 | #include "SphereDemo.h" |
tecnosys | 5:d2f1c9185c5b | 10 | |
tecnosys | 5:d2f1c9185c5b | 11 | #include <math.h> |
tecnosys | 5:d2f1c9185c5b | 12 | |
tecnosys | 5:d2f1c9185c5b | 13 | /****************************************************************************** |
tecnosys | 5:d2f1c9185c5b | 14 | * Typedefs and defines |
tecnosys | 5:d2f1c9185c5b | 15 | *****************************************************************************/ |
tecnosys | 5:d2f1c9185c5b | 16 | |
tecnosys | 5:d2f1c9185c5b | 17 | #define PI2 6.283185307179586476925286766559 |
tecnosys | 5:d2f1c9185c5b | 18 | |
tecnosys | 5:d2f1c9185c5b | 19 | /* Red color mask, 565 mode */ |
tecnosys | 5:d2f1c9185c5b | 20 | #define REDMASK 0xF800 |
tecnosys | 5:d2f1c9185c5b | 21 | /* Red shift value, 565 mode */ |
tecnosys | 5:d2f1c9185c5b | 22 | #define REDSHIFT 11 |
tecnosys | 5:d2f1c9185c5b | 23 | /* Green color mask, 565 mode */ |
tecnosys | 5:d2f1c9185c5b | 24 | #define GREENMASK 0x07E0 |
tecnosys | 5:d2f1c9185c5b | 25 | /* Green shift value, 565 mode */ |
tecnosys | 5:d2f1c9185c5b | 26 | #define GREENSHIFT 5 |
tecnosys | 5:d2f1c9185c5b | 27 | /* Blue color mask, 565 mode */ |
tecnosys | 5:d2f1c9185c5b | 28 | #define BLUEMASK 0x001F |
tecnosys | 5:d2f1c9185c5b | 29 | /* Blue shift value, 565 mode */ |
tecnosys | 5:d2f1c9185c5b | 30 | #define BLUESHIFT 0 |
tecnosys | 5:d2f1c9185c5b | 31 | |
tecnosys | 5:d2f1c9185c5b | 32 | /* Number of colors in 565 mode */ |
tecnosys | 5:d2f1c9185c5b | 33 | #define NUM_COLORS 65536 |
tecnosys | 5:d2f1c9185c5b | 34 | /* Number of red colors in 565 mode */ |
tecnosys | 5:d2f1c9185c5b | 35 | #define RED_COLORS 0x20 |
tecnosys | 5:d2f1c9185c5b | 36 | /* Number of green colors in 565 mode */ |
tecnosys | 5:d2f1c9185c5b | 37 | #define GREEN_COLORS 0x40 |
tecnosys | 5:d2f1c9185c5b | 38 | /* Number of blue colors in 565 mode */ |
tecnosys | 5:d2f1c9185c5b | 39 | #define BLUE_COLORS 0x20 |
tecnosys | 5:d2f1c9185c5b | 40 | |
tecnosys | 5:d2f1c9185c5b | 41 | /****************************************************************************** |
tecnosys | 5:d2f1c9185c5b | 42 | * Local variables |
tecnosys | 5:d2f1c9185c5b | 43 | *****************************************************************************/ |
tecnosys | 5:d2f1c9185c5b | 44 | |
tecnosys | 5:d2f1c9185c5b | 45 | |
tecnosys | 5:d2f1c9185c5b | 46 | /****************************************************************************** |
tecnosys | 5:d2f1c9185c5b | 47 | * External variables |
tecnosys | 5:d2f1c9185c5b | 48 | *****************************************************************************/ |
tecnosys | 5:d2f1c9185c5b | 49 | //extern EaLcdBoard lcdBoard; |
tecnosys | 5:d2f1c9185c5b | 50 | Display* _disp; |
tecnosys | 5:d2f1c9185c5b | 51 | extern bool abortTest; |
tecnosys | 5:d2f1c9185c5b | 52 | |
tecnosys | 5:d2f1c9185c5b | 53 | /****************************************************************************** |
tecnosys | 5:d2f1c9185c5b | 54 | * Local functions |
tecnosys | 5:d2f1c9185c5b | 55 | *****************************************************************************/ |
tecnosys | 5:d2f1c9185c5b | 56 | |
tecnosys | 5:d2f1c9185c5b | 57 | uint16_t SphereDemo::fastsqrt(uint32_t n) const { |
tecnosys | 5:d2f1c9185c5b | 58 | uint16_t c = 0x8000; |
tecnosys | 5:d2f1c9185c5b | 59 | uint16_t g = 0x8000; |
tecnosys | 5:d2f1c9185c5b | 60 | for(;;) { |
tecnosys | 5:d2f1c9185c5b | 61 | if(g*g > n) |
tecnosys | 5:d2f1c9185c5b | 62 | g ^= c; |
tecnosys | 5:d2f1c9185c5b | 63 | c >>= 1; |
tecnosys | 5:d2f1c9185c5b | 64 | if(c == 0) |
tecnosys | 5:d2f1c9185c5b | 65 | return g; |
tecnosys | 5:d2f1c9185c5b | 66 | g |= c; |
tecnosys | 5:d2f1c9185c5b | 67 | } |
tecnosys | 5:d2f1c9185c5b | 68 | } |
tecnosys | 5:d2f1c9185c5b | 69 | |
tecnosys | 5:d2f1c9185c5b | 70 | void SphereDemo::matrix(int16_t xyz[3][N], uint8_t rgb[3][N]) { |
tecnosys | 5:d2f1c9185c5b | 71 | static uint32_t t = 0; |
tecnosys | 5:d2f1c9185c5b | 72 | uint16_t i; |
tecnosys | 5:d2f1c9185c5b | 73 | int16_t x = -SCALE; |
tecnosys | 5:d2f1c9185c5b | 74 | int16_t y = -SCALE; |
tecnosys | 5:d2f1c9185c5b | 75 | uint16_t d; |
tecnosys | 5:d2f1c9185c5b | 76 | uint16_t s; |
tecnosys | 5:d2f1c9185c5b | 77 | |
tecnosys | 5:d2f1c9185c5b | 78 | for(i = 0; i < N; i++) { |
tecnosys | 5:d2f1c9185c5b | 79 | |
tecnosys | 5:d2f1c9185c5b | 80 | xyz[0][i] = x; |
tecnosys | 5:d2f1c9185c5b | 81 | xyz[1][i] = y; |
tecnosys | 5:d2f1c9185c5b | 82 | |
tecnosys | 5:d2f1c9185c5b | 83 | d = fastsqrt(x * x + y * y); |
tecnosys | 5:d2f1c9185c5b | 84 | s = sine[(t * 30) % SCALE] + SCALE; |
tecnosys | 5:d2f1c9185c5b | 85 | |
tecnosys | 5:d2f1c9185c5b | 86 | xyz[2][i] = sine[(d + s) % SCALE] * |
tecnosys | 5:d2f1c9185c5b | 87 | sine[(t * 10) % SCALE] / SCALE / 2; |
tecnosys | 5:d2f1c9185c5b | 88 | |
tecnosys | 5:d2f1c9185c5b | 89 | rgb[0][i] = (cosi[xyz[2][i] + SCALE / 2] + SCALE) * |
tecnosys | 5:d2f1c9185c5b | 90 | (RED_COLORS - 1) / SCALE / 2; |
tecnosys | 5:d2f1c9185c5b | 91 | |
tecnosys | 5:d2f1c9185c5b | 92 | rgb[1][i] = (cosi[(xyz[2][i] + SCALE / 2 + 2 * SCALE / 3) % SCALE] + SCALE) * |
tecnosys | 5:d2f1c9185c5b | 93 | (GREEN_COLORS - 1) / SCALE / 2; |
tecnosys | 5:d2f1c9185c5b | 94 | |
tecnosys | 5:d2f1c9185c5b | 95 | rgb[2][i] = (cosi[(xyz[2][i] + SCALE / 2 + SCALE / 3) % SCALE] + SCALE) * |
tecnosys | 5:d2f1c9185c5b | 96 | (BLUE_COLORS - 1) / SCALE / 2; |
tecnosys | 5:d2f1c9185c5b | 97 | |
tecnosys | 5:d2f1c9185c5b | 98 | x += INCREMENT; |
tecnosys | 5:d2f1c9185c5b | 99 | if(x >= SCALE) { |
tecnosys | 5:d2f1c9185c5b | 100 | x = -SCALE; |
tecnosys | 5:d2f1c9185c5b | 101 | y += INCREMENT; |
tecnosys | 5:d2f1c9185c5b | 102 | } |
tecnosys | 5:d2f1c9185c5b | 103 | |
tecnosys | 5:d2f1c9185c5b | 104 | } |
tecnosys | 5:d2f1c9185c5b | 105 | t++; |
tecnosys | 5:d2f1c9185c5b | 106 | } |
tecnosys | 5:d2f1c9185c5b | 107 | |
tecnosys | 5:d2f1c9185c5b | 108 | void SphereDemo::rotate(int16_t xyz[3][N], uint8_t rgb[3][N], |
tecnosys | 5:d2f1c9185c5b | 109 | uint16_t angleX, uint16_t angleY, uint16_t angleZ) { |
tecnosys | 5:d2f1c9185c5b | 110 | uint16_t i; |
tecnosys | 5:d2f1c9185c5b | 111 | int16_t tmpX; |
tecnosys | 5:d2f1c9185c5b | 112 | int16_t tmpY; |
tecnosys | 5:d2f1c9185c5b | 113 | int16_t sinx = sine[angleX]; |
tecnosys | 5:d2f1c9185c5b | 114 | int16_t cosx = cosi[angleX]; |
tecnosys | 5:d2f1c9185c5b | 115 | int16_t siny = sine[angleY]; |
tecnosys | 5:d2f1c9185c5b | 116 | int16_t cosy = cosi[angleY]; |
tecnosys | 5:d2f1c9185c5b | 117 | int16_t sinz = sine[angleZ]; |
tecnosys | 5:d2f1c9185c5b | 118 | int16_t cosz = cosi[angleZ]; |
tecnosys | 5:d2f1c9185c5b | 119 | |
tecnosys | 5:d2f1c9185c5b | 120 | for(i = 0; i < N; i++) { |
tecnosys | 5:d2f1c9185c5b | 121 | tmpX = (xyz[0][i] * cosx - xyz[2][i] * sinx) / SCALE; |
tecnosys | 5:d2f1c9185c5b | 122 | xyz[2][i] = (xyz[0][i] * sinx + xyz[2][i] * cosx) / SCALE; |
tecnosys | 5:d2f1c9185c5b | 123 | xyz[0][i] = tmpX; |
tecnosys | 5:d2f1c9185c5b | 124 | |
tecnosys | 5:d2f1c9185c5b | 125 | tmpY = (xyz[1][i] * cosy - xyz[2][i] * siny) / SCALE; |
tecnosys | 5:d2f1c9185c5b | 126 | xyz[2][i] = (xyz[1][i] * siny + xyz[2][i] * cosy) / SCALE; |
tecnosys | 5:d2f1c9185c5b | 127 | xyz[1][i] = tmpY; |
tecnosys | 5:d2f1c9185c5b | 128 | |
tecnosys | 5:d2f1c9185c5b | 129 | tmpX = (xyz[0][i] * cosz - xyz[1][i] * sinz) / SCALE; |
tecnosys | 5:d2f1c9185c5b | 130 | xyz[1][i] = (xyz[0][i] * sinz + xyz[1][i] * cosz) / SCALE; |
tecnosys | 5:d2f1c9185c5b | 131 | xyz[0][i] = tmpX; |
tecnosys | 5:d2f1c9185c5b | 132 | } |
tecnosys | 5:d2f1c9185c5b | 133 | } |
tecnosys | 5:d2f1c9185c5b | 134 | |
tecnosys | 5:d2f1c9185c5b | 135 | void SphereDemo::draw(int16_t xyz[3][N], uint8_t rgb[3][N]) { |
tecnosys | 5:d2f1c9185c5b | 136 | // static uint16_t oldProjX[N] = {0}; |
tecnosys | 5:d2f1c9185c5b | 137 | // static uint16_t oldProjY[N] = {0}; |
tecnosys | 5:d2f1c9185c5b | 138 | // static uint8_t oldDotSize[N] = {0}; |
tecnosys | 5:d2f1c9185c5b | 139 | uint16_t i; |
tecnosys | 5:d2f1c9185c5b | 140 | uint16_t projX; |
tecnosys | 5:d2f1c9185c5b | 141 | uint16_t projY; |
tecnosys | 5:d2f1c9185c5b | 142 | uint16_t projZ; |
tecnosys | 5:d2f1c9185c5b | 143 | uint16_t dotSize; |
tecnosys | 5:d2f1c9185c5b | 144 | static uint8_t cnt=0; |
tecnosys | 5:d2f1c9185c5b | 145 | |
tecnosys | 5:d2f1c9185c5b | 146 | if (cnt == 0) |
tecnosys | 5:d2f1c9185c5b | 147 | { |
tecnosys | 5:d2f1c9185c5b | 148 | cnt = 1; |
tecnosys | 5:d2f1c9185c5b | 149 | pFrmBuf = pFrmBuf1; |
tecnosys | 5:d2f1c9185c5b | 150 | } |
tecnosys | 5:d2f1c9185c5b | 151 | else if (cnt == 1) |
tecnosys | 5:d2f1c9185c5b | 152 | { |
tecnosys | 5:d2f1c9185c5b | 153 | cnt = 2; |
tecnosys | 5:d2f1c9185c5b | 154 | pFrmBuf = pFrmBuf2; |
tecnosys | 5:d2f1c9185c5b | 155 | } |
tecnosys | 5:d2f1c9185c5b | 156 | else |
tecnosys | 5:d2f1c9185c5b | 157 | { |
tecnosys | 5:d2f1c9185c5b | 158 | cnt = 0; |
tecnosys | 5:d2f1c9185c5b | 159 | pFrmBuf = pFrmBuf3; |
tecnosys | 5:d2f1c9185c5b | 160 | } |
tecnosys | 5:d2f1c9185c5b | 161 | |
tecnosys | 5:d2f1c9185c5b | 162 | graphics.setFrameBuffer(pFrmBuf); |
tecnosys | 5:d2f1c9185c5b | 163 | |
tecnosys | 5:d2f1c9185c5b | 164 | memset((void*)(pFrmBuf), 0x00, windowX * windowY * 2); |
tecnosys | 5:d2f1c9185c5b | 165 | |
tecnosys | 5:d2f1c9185c5b | 166 | for(i = 0; i < N; i++) { |
tecnosys | 5:d2f1c9185c5b | 167 | projZ = SCALE - (xyz[2][i] + SCALE) / 4; //4; |
tecnosys | 5:d2f1c9185c5b | 168 | projX = windowX / 2 + (xyz[0][i] * projZ / SCALE) / 40; //EA 25; |
tecnosys | 5:d2f1c9185c5b | 169 | projY = windowY / 2 + (xyz[1][i] * projZ / SCALE) / 40; //EA 25; |
tecnosys | 5:d2f1c9185c5b | 170 | dotSize = 3 - (xyz[2][i] + SCALE) * 2 / SCALE; |
tecnosys | 5:d2f1c9185c5b | 171 | //EA put_circle(oldProjX[i], oldProjY[i], 0, dotSize, 1); |
tecnosys | 5:d2f1c9185c5b | 172 | |
tecnosys | 5:d2f1c9185c5b | 173 | if((projX > dotSize) && |
tecnosys | 5:d2f1c9185c5b | 174 | (projY > dotSize) && |
tecnosys | 5:d2f1c9185c5b | 175 | (projX < (windowX - dotSize)) && |
tecnosys | 5:d2f1c9185c5b | 176 | (projY < (windowY - dotSize))) { |
tecnosys | 5:d2f1c9185c5b | 177 | |
tecnosys | 5:d2f1c9185c5b | 178 | graphics.put_circle(projX, projY, (rgb[0][i] << REDSHIFT) + (rgb[1][i] << GREENSHIFT) + (rgb[2][i] << BLUESHIFT), dotSize, 1); |
tecnosys | 5:d2f1c9185c5b | 179 | |
tecnosys | 5:d2f1c9185c5b | 180 | // oldProjX[i] = projX; |
tecnosys | 5:d2f1c9185c5b | 181 | // oldProjY[i] = projY; |
tecnosys | 5:d2f1c9185c5b | 182 | // oldDotSize[i] = dotSize; |
tecnosys | 5:d2f1c9185c5b | 183 | } |
tecnosys | 5:d2f1c9185c5b | 184 | } |
tecnosys | 5:d2f1c9185c5b | 185 | } |
tecnosys | 5:d2f1c9185c5b | 186 | |
tecnosys | 5:d2f1c9185c5b | 187 | |
tecnosys | 5:d2f1c9185c5b | 188 | |
tecnosys | 5:d2f1c9185c5b | 189 | /****************************************************************************** |
tecnosys | 5:d2f1c9185c5b | 190 | * Public functions |
tecnosys | 5:d2f1c9185c5b | 191 | *****************************************************************************/ |
tecnosys | 5:d2f1c9185c5b | 192 | SphereDemo::SphereDemo(void *pFrameBuf, int dispWidth, int dispHeight) |
tecnosys | 5:d2f1c9185c5b | 193 | : graphics((uint16_t *)pFrameBuf, dispWidth, dispHeight) |
tecnosys | 5:d2f1c9185c5b | 194 | { |
tecnosys | 5:d2f1c9185c5b | 195 | this->windowX = dispWidth; |
tecnosys | 5:d2f1c9185c5b | 196 | this->windowY = dispHeight; |
tecnosys | 5:d2f1c9185c5b | 197 | this->pFrmBuf = (uint16_t *)pFrameBuf; |
tecnosys | 5:d2f1c9185c5b | 198 | this->pFrmBuf1 = (uint16_t *)pFrameBuf; |
tecnosys | 5:d2f1c9185c5b | 199 | this->pFrmBuf2 = (uint16_t *)((uint32_t)pFrameBuf + dispWidth*dispHeight*2); |
tecnosys | 5:d2f1c9185c5b | 200 | this->pFrmBuf3 = (uint16_t *)((uint32_t)pFrameBuf + dispWidth*dispHeight*4); |
tecnosys | 5:d2f1c9185c5b | 201 | |
tecnosys | 5:d2f1c9185c5b | 202 | for(uint16_t i = 0; i < SCALE; i++) { |
tecnosys | 5:d2f1c9185c5b | 203 | sine[i] = (int)(sin(PI2 * i / SCALE) * SCALE); |
tecnosys | 5:d2f1c9185c5b | 204 | cosi[i] = (int)(cos(PI2 * i / SCALE) * SCALE); |
tecnosys | 5:d2f1c9185c5b | 205 | } |
tecnosys | 5:d2f1c9185c5b | 206 | } |
tecnosys | 5:d2f1c9185c5b | 207 | |
tecnosys | 5:d2f1c9185c5b | 208 | void SphereDemo::run(uint32_t loops, uint32_t delayMs) |
tecnosys | 5:d2f1c9185c5b | 209 | { |
tecnosys | 5:d2f1c9185c5b | 210 | printf("SphereDemo, %d loops, %dms delay\n", loops, delayMs); |
tecnosys | 5:d2f1c9185c5b | 211 | |
tecnosys | 5:d2f1c9185c5b | 212 | int16_t angleX = 0; |
tecnosys | 5:d2f1c9185c5b | 213 | int16_t angleY = 1000; |
tecnosys | 5:d2f1c9185c5b | 214 | int16_t angleZ = 0; |
tecnosys | 5:d2f1c9185c5b | 215 | |
tecnosys | 5:d2f1c9185c5b | 216 | int16_t speedX = 0; |
tecnosys | 5:d2f1c9185c5b | 217 | int16_t speedY = 0; |
tecnosys | 5:d2f1c9185c5b | 218 | int16_t speedZ = 0; |
tecnosys | 5:d2f1c9185c5b | 219 | |
tecnosys | 5:d2f1c9185c5b | 220 | int16_t xyz[3][N]; |
tecnosys | 5:d2f1c9185c5b | 221 | uint8_t rgb[3][N]; |
tecnosys | 5:d2f1c9185c5b | 222 | |
tecnosys | 5:d2f1c9185c5b | 223 | loops = 2*820; |
tecnosys | 5:d2f1c9185c5b | 224 | for(int32_t n=0;n<loops;n++) { |
tecnosys | 5:d2f1c9185c5b | 225 | |
tecnosys | 5:d2f1c9185c5b | 226 | matrix(xyz, rgb); |
tecnosys | 5:d2f1c9185c5b | 227 | |
tecnosys | 5:d2f1c9185c5b | 228 | rotate(xyz, rgb, angleX, angleY, angleZ); |
tecnosys | 5:d2f1c9185c5b | 229 | |
tecnosys | 5:d2f1c9185c5b | 230 | draw(xyz, rgb); |
tecnosys | 5:d2f1c9185c5b | 231 | //update framebuffer |
tecnosys | 5:d2f1c9185c5b | 232 | _disp = DMBoard::instance().display(); |
tecnosys | 5:d2f1c9185c5b | 233 | |
tecnosys | 5:d2f1c9185c5b | 234 | _disp->swapFramebuffer(this->pFrmBuf); |
tecnosys | 5:d2f1c9185c5b | 235 | //lcdBoard.setFrameBuffer((uint32_t)this->pFrmBuf); |
tecnosys | 5:d2f1c9185c5b | 236 | |
tecnosys | 5:d2f1c9185c5b | 237 | #if 0 |
tecnosys | 5:d2f1c9185c5b | 238 | if(joyState & JOYSTICK_RIGHT) |
tecnosys | 5:d2f1c9185c5b | 239 | speedX -= SPEED; |
tecnosys | 5:d2f1c9185c5b | 240 | else if(joyState & JOYSTICK_LEFT) |
tecnosys | 5:d2f1c9185c5b | 241 | speedX += SPEED; |
tecnosys | 5:d2f1c9185c5b | 242 | else if(joyState & JOYSTICK_UP) |
tecnosys | 5:d2f1c9185c5b | 243 | speedY -= SPEED; |
tecnosys | 5:d2f1c9185c5b | 244 | else if(joyState & JOYSTICK_DOWN) |
tecnosys | 5:d2f1c9185c5b | 245 | speedY += SPEED; |
tecnosys | 5:d2f1c9185c5b | 246 | else if(ledState & KEY1) |
tecnosys | 5:d2f1c9185c5b | 247 | speedZ -= SPEED; |
tecnosys | 5:d2f1c9185c5b | 248 | else if(ledState & KEY2) |
tecnosys | 5:d2f1c9185c5b | 249 | speedZ += SPEED; |
tecnosys | 5:d2f1c9185c5b | 250 | else if(ledState & KEY3) { |
tecnosys | 5:d2f1c9185c5b | 251 | speedX = 0; |
tecnosys | 5:d2f1c9185c5b | 252 | speedY = 0; |
tecnosys | 5:d2f1c9185c5b | 253 | speedZ = 0; |
tecnosys | 5:d2f1c9185c5b | 254 | angleX = 0; |
tecnosys | 5:d2f1c9185c5b | 255 | angleY = 0; |
tecnosys | 5:d2f1c9185c5b | 256 | angleZ = 0; |
tecnosys | 5:d2f1c9185c5b | 257 | } else |
tecnosys | 5:d2f1c9185c5b | 258 | #endif |
tecnosys | 5:d2f1c9185c5b | 259 | { |
tecnosys | 5:d2f1c9185c5b | 260 | if(speedX > 0) |
tecnosys | 5:d2f1c9185c5b | 261 | speedX -= SPEED; |
tecnosys | 5:d2f1c9185c5b | 262 | else if(speedX < 0) |
tecnosys | 5:d2f1c9185c5b | 263 | speedX += SPEED; |
tecnosys | 5:d2f1c9185c5b | 264 | |
tecnosys | 5:d2f1c9185c5b | 265 | if(speedY > 0) |
tecnosys | 5:d2f1c9185c5b | 266 | speedY -= SPEED; |
tecnosys | 5:d2f1c9185c5b | 267 | else if(speedY < 0) |
tecnosys | 5:d2f1c9185c5b | 268 | speedY += SPEED; |
tecnosys | 5:d2f1c9185c5b | 269 | |
tecnosys | 5:d2f1c9185c5b | 270 | if(speedZ > 0) |
tecnosys | 5:d2f1c9185c5b | 271 | speedZ -= SPEED; |
tecnosys | 5:d2f1c9185c5b | 272 | else if(speedZ < 0) |
tecnosys | 5:d2f1c9185c5b | 273 | speedZ += SPEED; |
tecnosys | 5:d2f1c9185c5b | 274 | } |
tecnosys | 5:d2f1c9185c5b | 275 | |
tecnosys | 5:d2f1c9185c5b | 276 | angleX += 0; //speedX; |
tecnosys | 5:d2f1c9185c5b | 277 | angleY += 0; //speedY; |
tecnosys | 5:d2f1c9185c5b | 278 | angleZ += 2; //speedZ; |
tecnosys | 5:d2f1c9185c5b | 279 | |
tecnosys | 5:d2f1c9185c5b | 280 | if(angleX >= SCALE) |
tecnosys | 5:d2f1c9185c5b | 281 | angleX -= SCALE; |
tecnosys | 5:d2f1c9185c5b | 282 | else if(angleX < 0) |
tecnosys | 5:d2f1c9185c5b | 283 | angleX += SCALE; |
tecnosys | 5:d2f1c9185c5b | 284 | |
tecnosys | 5:d2f1c9185c5b | 285 | if(angleY >= SCALE) |
tecnosys | 5:d2f1c9185c5b | 286 | angleY -= SCALE; |
tecnosys | 5:d2f1c9185c5b | 287 | else if(angleY < 0) |
tecnosys | 5:d2f1c9185c5b | 288 | angleY += SCALE; |
tecnosys | 5:d2f1c9185c5b | 289 | |
tecnosys | 5:d2f1c9185c5b | 290 | if(angleZ >= SCALE) |
tecnosys | 5:d2f1c9185c5b | 291 | angleZ -= SCALE; |
tecnosys | 5:d2f1c9185c5b | 292 | else if(angleZ < 0) |
tecnosys | 5:d2f1c9185c5b | 293 | angleZ += SCALE; |
tecnosys | 5:d2f1c9185c5b | 294 | |
tecnosys | 5:d2f1c9185c5b | 295 | if (abortTest) { |
tecnosys | 5:d2f1c9185c5b | 296 | return; |
tecnosys | 5:d2f1c9185c5b | 297 | } |
tecnosys | 5:d2f1c9185c5b | 298 | } |
tecnosys | 5:d2f1c9185c5b | 299 | //wait_ms(delayMs); |
tecnosys | 5:d2f1c9185c5b | 300 | wait_ms(1000); |
tecnosys | 5:d2f1c9185c5b | 301 | |
tecnosys | 5:d2f1c9185c5b | 302 | } |
tecnosys | 5:d2f1c9185c5b | 303 |