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