20 #define _waitcnt(x) __builtin_propeller_waitcnt((x), 0)
24 #define INLINE__ static inline
25 #define Yield__() __asm__ volatile( "" ::: "memory" )
27 #define INLINE__ static
29 #define waitcnt(n) _waitcnt(n)
30 #define coginit(id, code, par) _coginit((unsigned)(par)>>2, (unsigned)(code)>>2, id)
31 #define cognew(code, par) coginit(0x8, (code), (par))
32 #define cogstop(i) _cogstop(i)
35 INLINE__ int32_t Min__(int32_t a, int32_t b) {
return a < b ? a : b; }
36 INLINE__ int32_t Max__(int32_t a, int32_t b) {
return a > b ? a : b; }
37 INLINE__ int32_t Shr__(uint32_t a, uint32_t b) {
return (a>>b); }
38 static uint32_t LFSR__(uint32_t
x, uint32_t forward) {
41 a = forward ? 0x8000000B : 0x17;
42 for (y = 0; y < 32; y++) {
43 c = __builtin_parity(
x & a);
44 if (forward)
x = (
x<<1) | c;
45 else x = (
x>>1) | (c<<31);
49 #define RandForw__(x) ((x) = LFSR__((x), 1))
50 #define RandBack__(x) ((x) = LFSR__((x), 0))
51 typedef void (*Cogfunc__)(
void *a,
void *b,
void *c,
void *d);
52 static void Cogstub__(
void *argp) {
53 void **arg = (
void **)argp;
54 Cogfunc__ func = (Cogfunc__)(arg[0]);
55 func(arg[1], arg[2], arg[3], arg[4]);
57 __asm__(
".global _cogstart\n");
58 extern void _clone_cog(
void *tmp) __attribute__((weak));
59 extern long _load_start_kernel[] __attribute__((weak));
60 static int32_t Coginit__(
int cogid,
void *stackbase,
size_t stacksize,
void *func, int32_t arg1, int32_t arg2, int32_t arg3, int32_t arg4) {
61 void *tmp = _load_start_kernel;
62 unsigned int *sp = ((
unsigned int *)stackbase) + stacksize/4;
63 static int32_t cogargs__[5];
65 cogargs__[0] = (int32_t) func;
71 tmp = __builtin_alloca(1984);
75 *--sp = (
unsigned int)cogargs__;
76 *--sp = (
unsigned int)Cogstub__;
81 static void s3_obstacler();
82 static int32_t s3_faultMonitor();
83 static int32_t s3_readBarWidth();
91 scribbler_start(&self->Scribbler);
92 scribbler_start_motors();
93 scribbler_start_tones(&self->Scribbler);
94 scribbler_button_mode(-1, -1);
95 scribbler_set_volume(S3_SPKR_VOL);
96 scribbler_set_voices(SCRIBBLER_SAW, SCRIBBLER_SAW);
97 self->RunningCog = __builtin_propeller_cogid() + 1;
98 Coginit__(30, (
void *)self->FMStack, 200, (
void *)s3_faultMonitor, 0, 0, 0, 0);
99 Coginit__(30, (
void *)self->stack, 120, (
void *)s3_obstacler, 0, 0, 0, 0);
100 _waitcnt((
CNT + 10000000));
105 scribbler_start_mic_env(&self->Scribbler);
110 int micVol = scribbler_get_mic_env();
113 if(micVol < 0) micVol = 0;
114 if(micVol > 100) micVol = 100;
118 static void s3_obstacler()
120 int32_t side, ObstacleThld;
121 if (scribbler_get_model_s3()) {
122 if (scribbler_get_obstacle_threshold() != S3_OBSTACLE_THLD) {
123 ObstacleThld = scribbler_get_obstacle_threshold();
125 ObstacleThld = S3_OBSTACLE_THLD;
128 if (scribbler_get_obstacle_threshold() != SCRIBBLER_DEFAULT_OBSTACLE_THLD) {
129 ObstacleThld = scribbler_get_obstacle_threshold();
131 ObstacleThld = SCRIBBLER_DEFAULT_OBSTACLE_THLD;
135 for(side = SCRIBBLER_OBS_TX_LEFT; side <= SCRIBBLER_OBS_TX_RIGHT; side = side + 9) {
136 FRQA = (14000 * ObstacleThld) + (20607 * (100 - ObstacleThld));
137 CTRA = (4 << 26) | side;
139 _waitcnt((
CNT + 24000));
140 self->obs[((side == SCRIBBLER_OBS_TX_RIGHT) + 1)] = -(((
INA >> SCRIBBLER_OBS_RX) & 0x1) == 0);
141 DIRA &= (~(1 << side));
148 static int32_t s3_faultMonitor()
152 _waitcnt((
CNT + 80000000));
154 value = Min__(value, scribbler_get_adc_results(SCRIBBLER_ADC_VBAT));
157 scribbler_set_led(SCRIBBLER_POWER, SCRIBBLER_BLUE);
161 scribbler_set_led(SCRIBBLER_POWER, 32);
163 scribbler_set_led(SCRIBBLER_POWER, SCRIBBLER_BLINK_BLUE);
166 if (scribbler_get_adc_results(SCRIBBLER_ADC_IMOT) > 210) {
167 __builtin_propeller_cogstop((self->RunningCog - 1));
168 self->RunningCog = 0;
169 scribbler_stop_now();
170 scribbler_set_leds(SCRIBBLER_BLINK_RED, SCRIBBLER_BLINK_RED, SCRIBBLER_BLINK_RED, SCRIBBLER_OFF);
180 void s3_motorSet(int32_t LeftVelocity, int32_t RightVelocity, int32_t move_time)
182 LeftVelocity = Min__((Max__((-255), ((LeftVelocity * 255) / 100))), 255);
183 RightVelocity = Min__((Max__((-255), ((RightVelocity * 255) / 100))), 255);
185 move_time = Max__(move_time, 1);
187 scribbler_wheels_now(LeftVelocity, RightVelocity, move_time);
188 self->WasLeftVelocity = LeftVelocity;
189 self->WasRightVelocity = RightVelocity;
191 self->WasLeftVelocity = 0;
192 self->WasRightVelocity = 0;
193 scribbler_wait_stop();
195 self->WasLeftVelocity = LeftVelocity;
196 self->WasRightVelocity = RightVelocity;
199 void s3_motorSetDistance(int32_t left_distance, int32_t right_distance, int32_t max_speed)
201 left_distance = Min__((Max__((-10000), left_distance)), 10000);
202 right_distance = Min__((Max__((-10000), right_distance)), 10000);
203 max_speed = Min__((Max__(1, ((max_speed * 15) / 100))), 15);
204 scribbler_move_now(left_distance, right_distance, 0, max_speed, 0);
205 scribbler_wait_stop();
208 void s3_motorGotoXY(int32_t x_loc, int32_t y_loc, int32_t max_speed)
210 x_loc = Min__((Max__((-32768), x_loc)), 32767);
211 y_loc = Min__((Max__((-32768), y_loc)), 32767);
212 max_speed = Min__((Max__(1, ((max_speed * 15) / 100))), 15);
213 scribbler_set_speed(max_speed);
214 scribbler_move_to(x_loc, y_loc);
215 scribbler_wait_stop();
218 void s3_motorSetRotate(int32_t degrees, int32_t radius, int32_t max_speed)
223 degrees = Min__((Max__((-1080), degrees)), 1080);
224 radius = Min__((Max__((-4400), radius)), 4400);
225 max_speed = Min__((Max__(1, ((max_speed * 15) / 100))), 15);
226 scribbler_set_speed(max_speed);
227 scribbler_arc_deg_now(degrees, (-radius));
228 scribbler_wait_stop();
231 int32_t s3_motorsMoving()
233 return scribbler_moving();
236 int32_t s3_tailWheelMoving() {
237 return (((scribbler_motion() & 0xFF00) >> 8 < 10) ? 1 : 0);
240 void s3_simpleDrive(int32_t Direction, int32_t Speed)
242 int32_t LeftVelocity, RightVelocity;
245 LeftVelocity = RightVelocity = Speed;
247 case S3_SLIGHT_RIGHT:
248 LeftVelocity = Speed;
249 RightVelocity = (Speed * 3) / 4;
251 case S3_GENTLE_RIGHT:
252 LeftVelocity = Speed;
253 RightVelocity = Speed / 2;
256 LeftVelocity = Speed;
257 RightVelocity = Speed / 4;
260 LeftVelocity = (Speed * 3) / 4;
261 RightVelocity = Speed;
264 LeftVelocity = Speed / 2;
265 RightVelocity = Speed;
268 LeftVelocity = Speed / 4;
269 RightVelocity = Speed;
272 scribbler_wheels_now(LeftVelocity, RightVelocity, 0);
273 self->WasLeftVelocity = LeftVelocity;
274 self->WasRightVelocity = RightVelocity;
277 void s3_simpleSpin(int32_t Angle, int32_t Speed, int32_t Resume)
279 scribbler_set_speed(Speed);
280 scribbler_arc_deg_now((-Angle), 0);
281 scribbler_wait_stop();
284 scribbler_wheels_now(self->WasLeftVelocity, self->WasRightVelocity, 0);
289 scribbler_wheels_now(0, 0, 0);
290 self->WasLeftVelocity = 0;
291 self->WasRightVelocity = 0;
294 void s3_setVolume(int32_t Volume)
296 Volume = Min__((Max__(0, Volume)), 100);
297 scribbler_set_volume(Volume);
300 void s3_setVoices(int32_t voice1, int32_t voice2)
302 scribbler_set_voices(voice1, voice2);
306 void s3_playNote(int32_t freq1, int32_t freq2, int32_t duration)
308 scribbler_play_tone((Max__((duration - 1), 1)), freq1, freq2);
309 scribbler_play_tone(1, 0, 0);
310 scribbler_wait_sync(0);
313 void s3_simplePlay(int32_t Frequency, int32_t Duration, int32_t Volume)
315 Volume = Min__((Max__(0, Volume)), 100);
316 scribbler_set_volume(Volume);
317 scribbler_play_tone((Max__((Duration - 1), 1)), Frequency, 0);
318 scribbler_play_tone(1, 0, 0);
319 scribbler_wait_sync(0);
333 int32_t s3_readBars()
335 int32_t w0, w1, barcount, midwidth, t;
337 if ((scribbler_line_sensor(SCRIBBLER_LEFT, S3_BAR_THLD)) || (scribbler_line_sensor(SCRIBBLER_RIGHT, S3_BAR_THLD))) {
341 while (!(scribbler_line_sensor(SCRIBBLER_RIGHT, S3_BAR_THLD))) {
342 if ((
CNT - t) > 80000000) {
346 if (!((w1 = s3_readBarWidth(
self)))) {
349 if (!((w0 = s3_readBarWidth(
self)))) {
352 midwidth = Shr__((w0 + w1), 1);
353 for(barcount = 1; barcount <= 5; barcount++) {
355 result = result << 1;
360 if (!((w0 = s3_readBarWidth(
self)))) {
366 result = (__builtin_propeller_rev(result, 32 - 3)) | 0x8;
372 static int32_t s3_readBarWidth()
376 while (scribbler_line_sensor(SCRIBBLER_RIGHT, S3_BAR_THLD)) {
377 if ((
CNT - t) > 80000000) {
382 while (!(scribbler_line_sensor(SCRIBBLER_RIGHT, S3_BAR_THLD))) {
383 if ((
CNT - t) > 80000000) {
391 int32_t s3_readObstacle(int32_t Side)
393 if ((Side == SCRIBBLER_LEFT) || (Side == SCRIBBLER_RIGHT)) {
394 return self->obs[Side];
399 int32_t s3_simpleObstacle(int32_t Condition, int32_t Location)
404 if ((Condition == S3_IS) || (Condition == S3_IS_NOT)) {
405 obsL =
self->obs[S3_LEFT];
406 obsR =
self->obs[S3_RIGHT];
408 self->WasObs[S3_LEFT] = obsL;
409 self->WasObs[S3_RIGHT] = obsR;
411 }
else if ((Condition == S3_WAS) || (Condition == S3_WAS_NOT)) {
412 obsL =
self->WasObs[S3_LEFT];
413 obsR =
self->WasObs[S3_RIGHT];
418 if (obsL && obsR) result = 1;
421 if (obsL && !obsR) result = 1;
424 if (obsR && !obsL) result = 1;
427 if (obsL || obsR) result = 1;
431 if ((Condition == S3_WAS_NOT) || (Condition == S3_IS_NOT)) result = !result;
436 int32_t s3_lineSensor(int32_t Side)
438 if ((Side == S3_LEFT) || (Side == S3_RIGHT)) {
439 return scribbler_line_sensor(Side, (-1));
444 int32_t s3_simpleLine(int32_t Condition, int32_t Location, int32_t Color)
448 if (!(self->RunningCog)) {
452 if ((Condition == S3_IS) || (Condition == S3_IS_NOT)) {
453 self->WasLine[S3_LEFT] = scribbler_line_sensor(S3_LEFT, (-1));
454 self->WasLine[S3_RIGHT] = scribbler_line_sensor(S3_RIGHT, (-1));
456 if ((abs((self->WasLine[S3_LEFT] - self->WasLine[S3_RIGHT]))) < 30) {
458 if ((self->WasLine[S3_LEFT] + self->WasLine[S3_RIGHT]) < 60) {
460 if ((Color == S3_BLACK) && ((Location == S3_CENTER) || (Location == S3_DETECTED))) {
464 if ((Color == S3_WHITE) && ((Location == S3_CENTER) || (Location == S3_DETECTED))) {
471 if (Location == S3_DETECTED) {
474 if ((self->WasLine[S3_LEFT] > self->WasLine[S3_RIGHT]) && (((Location == S3_LEFT) && (Color == S3_BLACK)) || ((Location == S3_RIGHT) && (Color == S3_WHITE)))) {
478 if ((self->WasLine[S3_RIGHT] > self->WasLine[S3_LEFT]) && (((Location == S3_LEFT) && (Color == S3_WHITE)) || ((Location == S3_RIGHT) && (Color == S3_BLACK)))) {
485 if ((Condition == S3_IS_NOT) || (Condition == S3_WAS_NOT)) result = !result;
490 int32_t s3_lightSensor(int32_t Side)
492 if (((Side == SCRIBBLER_LEFT) || (Side == SCRIBBLER_CENTER)) || (Side == SCRIBBLER_RIGHT)) {
493 return ((scribbler_light_sensor(Side) * 100) / 256);
498 int32_t s3_simpleLight(int32_t Condition, int32_t Location)
501 if ((Condition == S3_IS) || (Condition == S3_IS_NOT)) {
502 self->WasLight[S3_LEFT] = scribbler_light_sensor(S3_LEFT);
503 self->WasLight[S3_RIGHT] = scribbler_light_sensor(S3_RIGHT);
504 self->WasLight[S3_CENTER] = scribbler_light_sensor(S3_CENTER);
506 if (((self->WasLight[S3_LEFT] > (self->WasLight[S3_RIGHT] + 50)) && (self->WasLight[S3_LEFT] > (self->WasLight[S3_CENTER] + 50))) || ((self->WasLight[S3_LEFT] > ((self->WasLight[S3_RIGHT] * 3) / 2)) && (self->WasLight[S3_LEFT] > ((self->WasLight[S3_CENTER] * 3) / 2)))) {
507 if (Location == S3_LEFT) {
511 if (((self->WasLight[S3_RIGHT] > (self->WasLight[S3_LEFT] + 50)) && (self->WasLight[S3_RIGHT] > (self->WasLight[S3_CENTER] + 50))) || ((self->WasLight[S3_RIGHT] > ((self->WasLight[S3_LEFT] * 3) / 2)) && (self->WasLight[S3_RIGHT] > ((self->WasLight[S3_CENTER] * 3) / 2)))) {
512 if (Location == S3_RIGHT) {
516 if (((self->WasLight[S3_CENTER] > (self->WasLight[S3_LEFT] + 50)) && (self->WasLight[S3_CENTER] > (self->WasLight[S3_RIGHT] + 50))) || ((self->WasLight[S3_CENTER] > ((self->WasLight[S3_LEFT] * 3) / 2)) && (self->WasLight[S3_CENTER] > ((self->WasLight[S3_RIGHT] * 3) / 2)))) {
517 if (Location == S3_CENTER) {
521 if ((Location == S3_DETECTED) && (((self->WasLight[S3_LEFT] + self->WasLight[S3_CENTER]) + self->WasLight[S3_RIGHT]) > 50)) {
527 if ((Condition == S3_IS_NOT) || (Condition == S3_WAS_NOT)) result = !result;
532 void s3_setLED(int32_t LED, int32_t Color)
534 if (((LED == SCRIBBLER_LEFT) || (LED == SCRIBBLER_CENTER)) || (LED == SCRIBBLER_RIGHT)) {
536 case S3_COLOR_FF0000:
537 case S3_COLOR_00FF00:
538 case S3_COLOR_FF7F00:
540 scribbler_set_led(LED, Color);
548 return scribbler_stalled();
551 int32_t s3_simpleStalled(int32_t Condition)
555 self->WasStalled = scribbler_stalled();
556 return self->WasStalled;
559 self->WasStalled = scribbler_stalled();
560 return !(
self->WasStalled);
563 return self->WasStalled;
566 return !(
self->WasStalled);
571 int32_t s3_simpleButton(int32_t Condition)
575 self->WasButton = scribbler_button_press();
576 return self->WasButton;
579 self->WasButton = scribbler_button_press();
580 return !(
self->WasButton);
583 return self->WasButton;
586 return !(
self->WasButton);
591 int32_t s3_simpleRandom(int32_t Condition)
595 self->WasRandom = s3_booleanRandom(
self);
596 return self->WasRandom;
599 self->WasRandom = s3_booleanRandom(
self);
600 return !(
self->WasRandom);
603 return self->WasRandom;
606 return !(
self->WasRandom);
611 int32_t s3_booleanRandom()
613 if ((RandForw__(self->seed)) & 0x1) {
619 int32_t s3_randomRange(int32_t A, int32_t B)
621 int32_t High, Low, Range;
642 if (Range == 2147483647) {
643 return (Low + ((RandForw__(self->seed)) & 2147483647));
646 return (Low + (((RandForw__(self->seed)) & 2147483647) / (2147483647 / (Range + 1))));
649 int32_t s3_buttonCount()
651 return scribbler_button_count();
654 int32_t s3_buttonPressed()
656 return scribbler_button_press();
659 int32_t s3_resetButtonCount()
661 return scribbler_reset_button_count();
664 int32_t s3_runWithoutResult(int32_t
input)
669 int32_t s3_ping(int32_t Pin)
671 int32_t MaxLoops, StartCnt, EndCnt;
673 if (!((0 <= Pin) && (Pin <= 5))) {
679 OUTA &= (~(1 << Pin));
680 DIRA &= (~(1 << Pin));
682 while ((--MaxLoops) && (!((
INA >> Pin) & 0x1))) {
690 while ((--MaxLoops) && ((
INA >> Pin) & 0x1)) {
697 _waitcnt((16000 +
CNT));
698 result = EndCnt - StartCnt;
699 if ((result < 9200) || (result > 1480000)) {
706 void s3_memoryWrite(int32_t Addr, int32_t Value) {
707 int AddrMax = (32768 - SCRIBBLER_EE_USER_AREA) / 4;
708 if(Addr >= 0 && Addr < AddrMax) {
709 scribbler_ee_write_byte((Addr * 4) + SCRIBBLER_EE_USER_AREA, Value);
713 int32_t s3_memoryRead(int32_t Addr) {
714 return scribbler_ee_read_byte((Addr * 4) + SCRIBBLER_EE_USER_AREA);
718 int32_t s3_readADC(int32_t p) {
719 return (scribbler_get_results(p) * 500) >> 16;