uint8_t test_motor(uint8_t ev, uint16_t lparam, void* rparam) { switch(ev) { case EVENT_WINDOW_CREATED: data = 0; break; case EVENT_KEY_PRESSED: { switch(lparam) { case KEY_UP: data++; if (data > 16) data = 1; break; case KEY_DOWN: data--; if (data == 0) data = 16; break; case KEY_ENTER: data = 0; break; } motor_on(data, 0); window_invalid(NULL); break; } case EVENT_WINDOW_PAINT: { char buf[32]; tContext *pContext = (tContext*)rparam; GrContextForegroundSet(pContext, ClrBlack); GrRectFill(pContext, &client_clip); GrContextForegroundSet(pContext, ClrWhite); GrContextFontSet(pContext, (tFont*)&g_sFontGothic18); GrStringDraw(pContext, "Test Motor", -1, 32, 50, 0); sprintf(buf, "Motor Level: %d", data); GrStringDraw(pContext, buf, -1, 5, 70, 0); window_button(pContext, KEY_UP, "+"); window_button(pContext, KEY_DOWN, "-"); window_button(pContext, KEY_ENTER, "Reset"); break; } case EVENT_EXIT_PRESSED: motor_on(0, 0); return 0; // return 0 to close the window default: return 0; } return 1; }
//------------------------------------------------------------------------------ // Copy flash function to RAM. //------------------------------------------------------------------------------ void Upgrade(void) { unsigned char *flash_start_ptr; // Initialize pointers unsigned char *flash_end_ptr; unsigned char *RAM_start_ptr; if (CheckUpgrade() != 0xff) return; //Initialize flash and ram start and end address flash_start_ptr = (unsigned char *)__segment_begin("FLASHCODE"); flash_end_ptr = (unsigned char *)__segment_end("FLASHCODE"); RAM_start_ptr = (unsigned char *)__segment_begin("RAMCODE"); //calculate function size unsigned long function_size = (unsigned long)(flash_end_ptr) - (unsigned long)(flash_start_ptr); // Copy flash function to RAM printf("Copy From %p to %p size=%ld\n", flash_start_ptr, RAM_start_ptr, function_size); memcpy(RAM_start_ptr,flash_start_ptr,function_size); motor_on(0, 0); printf("Jump to %p\n", FlashFirmware); // remove the flag of firmware struct _header h; SPI_FLASH_BufferRead((void*)&h, FIRMWARE_BASE, sizeof(h)); SPI_FLASH_BufferWrite((void*)&h, FIRMWARE_BASE + h.length + 2 * sizeof(h), sizeof(h)); FlashFirmware(); }
static void blinker(void) { led_inv(RIGHT); led_inv(LEFT); #ifdef MATRIX for(uint8_t x = 0; x < 3; ++x) { for(uint8_t y = 0; y < 3; ++y) { matrix_set(x, y, ((x + y) & 1) ^ inv); } } inv = !inv; #endif wait_ms(500); if(button_clicked(RIGHT)) { motor_on(); } if(button_clicked(LEFT)) { motor_off(); } }
/* recalibrate the drive */ static void recalibrate(void) { //LOG("recalibrate() called ...\n"); /*turn the motor on first */ motor_on(); /* send actual command bytes */ send_byte(FD_RECALIBRATE); send_byte(0); /* wait until seek finished */ wait_fdc(TRUE); }
void window_notify(const char* title, const char* msg, uint8_t buttons, char icon) { message_title = title; message_subtitle = NULL; message = msg; message_buttons = buttons; message_icon = icon; skip = 0; push_uid(SPECIAL, 0); selectidx = 0; motor_on(50, CLOCK_SECOND); backlight_on(window_readconfig()->light_level, CLOCK_SECOND * 3); if (state & STATE_ACTIVE) window_invalid(NULL); else window_open(notify_process, NULL); }
//Read the string and execute instructions void process_string(uint8_t *instruction) { uint8_t code; uint16_t k; float temp; //command commands = NULL; FloatPoint fp; //the character / means delete block... used for comments and stuff. if (instruction[0] == '/') { Serial.println("ok"); return; } enable_steppers(); purge_commands(); //clear old commands parse_commands(instruction); //create linked list of arguments if (command_exists('G')) { code = getValue('G'); switch(code) { case 0: //Rapid Motion setXYZ(&fp); set_target(&fp); r_move(0); //fast motion in all axis break; case 1: //Coordinated Motion setXYZ(&fp); set_target(&fp); if (command_exists('F')) _feedrate = getValue('F'); //feedrate persists till changed. r_move( _feedrate ); break; case 2://Clockwise arc case 3://Counterclockwise arc FloatPoint cent; float angleA, angleB, angle, radius, length, aX, aY, bX, bY; //Set fp Values setXYZ(&fp); // Centre coordinates are always relative cent.x = xaxis->current_units + getValue('I'); cent.y = yaxis->current_units + getValue('J'); aX = (xaxis->current_units - cent.x); aY = (yaxis->current_units - cent.y); bX = (fp.x - cent.x); bY = (fp.y - cent.y); if (code == 2) { // Clockwise angleA = atan2(bY, bX); angleB = atan2(aY, aX); } else { // Counterclockwise angleA = atan2(aY, aX); angleB = atan2(bY, bX); } // Make sure angleB is always greater than angleA // and if not add 2PI so that it is (this also takes // care of the special case of angleA == angleB, // ie we want a complete circle) if (angleB <= angleA) angleB += 2 * M_PI; angle = angleB - angleA; radius = sqrt(aX * aX + aY * aY); length = radius * angle; int steps, s, step; steps = (int) ceil(length / curve_section); FloatPoint newPoint; for (s = 1; s <= steps; s++) { step = (code == 3) ? s : steps - s; // Work backwards for CW newPoint.x = cent.x + radius * cos(angleA + angle * ((float) step / steps)); newPoint.y = cent.y + radius * sin(angleA + angle * ((float) step / steps)); newPoint.z = zaxis->current_units; set_target(&newPoint); // Need to calculate rate for each section of curve feedrate_micros = (feedrate > 0) ? feedrate : getMaxFeedrate(); // Make step r_move(feedrate_micros); } break; case 4: //Dwell //delay((int)getValue('P')); break; case 20: //Inches for Units _units[0] = X_STEPS_PER_INCH; _units[1] = Y_STEPS_PER_INCH; _units[2] = Z_STEPS_PER_INCH; curve_section = CURVE_SECTION_INCHES; calculate_deltas(); break; case 21: //mm for Units _units[0] = X_STEPS_PER_MM; _units[1] = Y_STEPS_PER_MM; _units[2] = Z_STEPS_PER_MM; curve_section = CURVE_SECTION_MM; calculate_deltas(); break; case 28: //go home. set_target(&zeros); r_move(getMaxFeedrate()); break; case 30://go home via an intermediate point. //Set Target setXYZ(&fp); set_target(&fp); //go there. r_move(getMaxFeedrate()); //go home. set_target(&zeros); r_move(getMaxFeedrate()); break; case 81: // drilling operation temp = zaxis->current_units; //Move only in the XY direction setXYZ(&fp); set_target(&fp); zaxis->target_units = temp; calculate_deltas(); r_move(getMaxFeedrate()); //Drill DOWN zaxis->target_units = getValue('Z') + ((abs_mode) ? 0 : zaxis->current_units); calculate_deltas(); r_move(getMaxFeedrate()); //Drill UP zaxis->target_units = temp; calculate_deltas(); r_move(getMaxFeedrate()); case 90://Absolute Positioning abs_mode = true; break; case 91://Incremental Positioning abs_mode = false; break; case 92://Set as home set_position(&zeros); break; case 93://Inverse Time Feed Mode break; //TODO: add this case 94://Feed per Minute Mode break; //TODO: add this default: Serial.print("huh? G"); Serial.println(code,DEC); } } if (command_exists('M')) { code = getValue('M'); switch(code) { case 3: // turn on motor case 4: motor_on(); break; case 5: // turn off motor motor_off(); break; case 82: DDRC |= _BV(1); PORTC &= ~_BV(1); DDRC &= ~_BV(0); PORTC |= _BV(0); // setup initial position for (int i=0; i<20; i++) { k=0; PORTB |= _BV(5); //go down while(PINC & _BV(0)) { PORTB |= _BV(2); delayMicroseconds(1); PORTB &= ~_BV(2); delayMicroseconds(200); k++; } //print result for this point Serial.println(k,DEC); PORTB &= ~_BV(5); //move up to origin while (k--) { PORTB |= _BV(2); delayMicroseconds(1); PORTB &= ~_BV(2); delayMicroseconds(12.5*stepping); } } break; case 81: DDRC |= _BV(1); PORTC &= ~_BV(1); DDRC &= ~_BV(0); PORTC |= _BV(0); while(1) { if (PINC & _BV(0)) Serial.println("high"); else Serial.println("low"); } break; case 80: //plot out surface of milling area DDRC |= _BV(1); PORTC &= ~_BV(1); DDRC &= ~_BV(0); PORTC |= _BV(0); // setup initial position fp.x = 0; fp.y = 0; fp.z = 0; set_target(&fp); set_position(&fp); r_move(0); for (int i=0; i<160; i+=2) { for (float j=0; j<75; j+=2) { fp.x=i; fp.y=j; fp.z=0; set_target( &fp ); r_move( 0 ); k=0; PORTB &= ~(_BV(5)); //go down while(PINC & _BV(0)) { PORTB |= _BV(2); delayMicroseconds(1); PORTB &= ~_BV(2); delayMicroseconds(200); k++; } //print result for this point Serial.print(i,DEC); Serial.print(","); Serial.print(j,DEC); Serial.print(","); Serial.println(k,DEC); PORTB |= _BV(5); //move up to origin while (k--) { PORTB |= _BV(2); delayMicroseconds(1); PORTB &= ~_BV(2); delayMicroseconds(200); } } } break; case 90: //plot out surface of milling area DDRC |= _BV(1); PORTC &= ~_BV(1); DDRC &= ~_BV(0); PORTC |= _BV(0); // setup initial position fp.x = 0; fp.y = 135; fp.z = 0; set_target(&fp); set_position(&fp); r_move(0); for (int i=0; i<1; i++) { for (float j=135; j!=0; j-=.25) { fp.x=i; fp.y=j; fp.z=0; set_target( &fp ); r_move( 0 ); k=0; PORTB |= _BV(5); //go down while(PINC & _BV(0)) { PORTB |= _BV(2); delayMicroseconds(1); PORTB &= ~_BV(2); delayMicroseconds(200); k++; } //print result for this point Serial.print(i,DEC); Serial.print(","); Serial.print(j,DEC); Serial.print(","); Serial.println(k,DEC); PORTB &= ~_BV(5); //move up to origin while (k--) { PORTB |= _BV(2); delayMicroseconds(1); PORTB &= ~_BV(2); delayMicroseconds(200); } } } break; case 98: //M98 Find Z0 where it is one step from touching. DDRC |= _BV(1); PORTC &= ~_BV(1); DDRC &= ~_BV(0); PORTC |= _BV(0); PORTB |= _BV(5); while(PINC & _BV(0)) { PORTB |= _BV(2); delayMicroseconds(1); PORTB &= ~_BV(2); delayMicroseconds(200); } break; case 99: //M99 S{1,2,4,8,16} -- set stepping mode if (command_exists('S')) { code = getValue('S'); if (code == 1 || code == 2 || code == 4 || code == 8 || code == 16) { stepping = code; setStep(stepping); break; } } default: Serial.print("huh? M"); Serial.println(code,DEC); } } Serial.println("ok");//tell our host we're done. }
// notify window process static uint8_t notify_process(uint8_t ev, uint16_t lparam, void* rparam) { switch(ev) { case EVENT_WINDOW_CREATED: { state |= STATE_ACTIVE; add_watch_status(WS_NOTIFY); return 0x80; } case EVENT_WINDOW_PAINT: { onDraw((tContext*)rparam); break; } case EVENT_WINDOW_CLOSING: state &= ~STATE_ACTIVE; process_post(ui_process, EVENT_NOTIFY_RESULT, (void*)message_result); motor_on(0, 0); del_watch_status(WS_NOTIFY); selectidx = 0; num_uids = 0; return 0; break; case EVENT_KEY_PRESSED: if (lparam == KEY_DOWN) { if (state & STATE_MORE) { skip += 16; window_invalid(NULL); } else if (selectidx < num_uids) { selectidx++; fetch_content(); window_invalid(NULL); } } else if (lparam == KEY_UP) { if (skip >= 16) { skip-=16; window_invalid(NULL); } else if (skip == 0) { if (selectidx > 0) { selectidx--; fetch_content(); window_invalid(NULL); } } } else if (lparam == KEY_ENTER) { if (selectidx < num_uids - 1) { selectidx++; fetch_content(); window_invalid(NULL); } } break; default: return 0; } return 1; }
void window_notify_ancs(uint8_t command, uint32_t uid, uint8_t flag, uint8_t category) { if (command == 0) // add { if (lastmessageid != -1 && lastmessageid >= uid) { return; } message_title = NULL; message = NULL; push_uid(uid, (flag << 8) | category); selectidx = 0; motor_on(50, CLOCK_SECOND); backlight_on(window_readconfig()->light_level, CLOCK_SECOND * 3); lastmessageid = uid; if (state & STATE_ACTIVE) window_invalid(NULL); else window_open(notify_process, NULL); fetch_content(); } else if (command == 1) { if (state & STATE_ACTIVE) { // check if the current if (uids[0] == uid) { fetch_content(); } window_invalid(NULL); motor_on(50, CLOCK_SECOND); } } else if (command == 2) // remove { if (!(state & STATE_ACTIVE)) return; uint8_t refresh = 0; if (uids[0] == uid) { refresh = 1; } // find the item int i; for(i = 0; i < num_uids; i++) { if (uids[i] == uid) break; } if (i == num_uids) return; for (int j = i ; j < num_uids; j++) { uids[j] = uids[j+1]; attributes[j] = attributes[j+1]; } num_uids--; if (refresh) fetch_content(); } }
static void run(void) { if(ir_recv()) { // receive a ir signal, react motor_on(); led_on(RIGHT); set_note(NOTE_B, 5); wait_ms(200); set_note(NOTE_F, 5); wait_ms(100); led_off(RIGHT); led_on(LEFT); set_note(NOTE_G, 5); wait_ms(100); set_note(NOTE_Ab, 5); wait_ms(100); set_note(NOTE_A, 5); wait_ms(100); led_off(LEFT); motor_off(); } else if(button_clicked(RIGHT)) { // button clicked, send ir signal and do some stuff led_on(RIGHT); set_note(NOTE_A, 5); wait_ms(100); set_note(NOTE_Ab, 5); wait_ms(100); set_note(NOTE_G, 5); wait_ms(100); led_off(RIGHT); led_on(LEFT); set_note(NOTE_F, 5); wait_ms(100); set_note(NOTE_B, 5); wait_ms(200); stop_note(); led_off(LEFT); ir_on(); wait_ms(400); ir_off(); wait_ms(10); } else { // regular bug behaviour uint8_t light = photons_measure(); pentatonic_all_led_set(light >> 3); motor_set(biased_random(light) > BASELINE + 0x40); led_set(RIGHT, biased_random(light) > BASELINE + 0x00); led_set(LEFT, biased_random(light) > BASELINE + 0x00); if(biased_random(light) > BASELINE + 0x20) { uint16_t tone = (biased_random(light) * 2) + 500; set_note(tone, 0); } else { stop_note(); } wait_ms(200); } }
void main() { setup_adc_ports(NO_ANALOGS); setup_adc(ADC_CLOCK_DIV_2); setup_psp(PSP_DISABLED); setup_spi(SPI_SS_DISABLED); setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); setup_timer_1(T1_DISABLED); setup_timer_2(T2_DIV_BY_16,155,1); setup_ccp1(CCP_PWM); setup_ccp2(CCP_PWM); set_pwm1_duty(312); // Inicia el Ciclo de Trabajo PWM1 en 50%. set_pwm2_duty(312); // Inicia el Ciclo de Trabajo PWM2 en 50%. setup_comparator(NC_NC_NC_NC); setup_vref(FALSE); set_tris_a(0b11100000); // set_tris_c(0b10000000); //Pone RC7 como input y RC6 como output (y de 5 a 0 también) set_tris_b(0b00000000); // Habilita como salidas los pines B0, B1,...,B7 set_tris_e(0b010); // ************************ CONFIGURACIÓN PWM1 y PWM2: ************************ int32 brillo=0; int32 exposicion=500; //Tiempo de exposición de la cámara en [ms] int32 der_steps=0; int32 izq_steps=0; int32 led=0; int32 motor=0; int32 direccion=0; int32 pasos=0; int32 velocidad=0; char leido_pantalla[5]; output_low(PIN_B0); output_low(PIN_B1); output_low(PIN_B2); output_low(PIN_B3); output_low(PIN_B4); output_high(PIN_B6); // Siempre en 5V para conectar pull up 10kOhm de RA4 para SLEEP MOTOR 3 (altura) set_pwm1_duty(0); // Mantiene Ciclos en 0 para reducir consumo al iniciar. set_pwm2_duty(0); //*************** INICIO *************** while(true) { char seleccionar=0; output_low(PIN_A2); output_low(PIN_A3); output_low(PIN_A4); printf("Set parameters: e=exposicion(%Ld), v=velocidad(%Ld)\n\r",exposicion,velocidad); printf(" b=brillo(%Ld), d=direccion(%Ld), p=pasos(%Ld)\n\r",brillo,direccion,pasos); printf(" l=led(%Ld), m=motores(%Ld) \n\r",led,motor); seleccionar=getc(); switch(seleccionar) { case 'v': printf("Ingrese Velocidad en [ms] y [ENTER]\n\r"); fgets(leido_pantalla); velocidad=atoi32(leido_pantalla); break; case 'e': printf("Ingrese tiempo de exposicion en [ms] y [ENTER]\n\r"); fgets(leido_pantalla); exposicion=atoi32(leido_pantalla); break; case 'b': printf("Ingrese Ciclo de Trabajo para PWM1 (0-100) (brillo) y [ENTER]:\n\r"); fgets(leido_pantalla); brillo=atoi(leido_pantalla); set_pwm1_duty(brillo*20000000/(100*2000*16)); set_pwm2_duty(brillo*20000000/(100*2000*16)); break; case 'l': printf("Ingrese Led a encender: 0 a 7 y [ENTER]\n\r"); fgets(leido_pantalla); led=atoi32(leido_pantalla); break; case 'd': printf("Ingrese direccion 1=Derecha, 0=Izquierda y [ENTER]\n\r"); fgets(leido_pantalla); direccion = atoi32(leido_pantalla); break; case 'p': printf("Ingrese el numero de pasos a utlizar y [ENTER]\n\r"); fgets(leido_pantalla); pasos = atoi32(leido_pantalla); break; case 'm': printf("Ingrese el numero de motor a utlizar: 1,2 o 3 y [ENTER]\n\r"); fgets(leido_pantalla); motor = atoi32(leido_pantalla); break; case '1': led_on(led); break; case '2': led_off(); break; case '3': motor_move(motor,pasos,direccion); break; case '4': led_on_off(led,exposicion); break; case '5': int32 pasos_restantes; int32 steps; int dir; dir = direccion; steps = pasos; pasos_restantes = pasos; motor_on(motor); while(pasos_restantes > 0){ printf("pasos_restantes: %Ld\n\r",pasos_restantes); delay_us(200); steps = motores4(pasos_restantes,dir,velocidad); pasos_restantes = pasos_restantes - steps; if (pasos_restantes <=0) break; delay_us(200); dir = (dir == 0)?1:0; motores2(2000,dir); } break; case '6': int32 pasos_restantes2; int32 steps2; int dir2; dir2 = direccion; steps2 = pasos; pasos_restantes2 = pasos; motor_on(motor); while(true){ printf("pasos restantes: %Ld\n\r",pasos_restantes2); delay_us(200); steps2 = motores4(pasos_restantes2,dir2,velocidad); delay_us(200); dir2 = (dir2 == 0)?1:0; motores2(2000,dir2); pasos_restantes2 = pasos_restantes2 - steps2; if (pasos_restantes2 <=0) pasos_restantes2 = pasos; } break; case '7': int32 steps3; motor_on(motor); steps3 = motores4(pasos,direccion,velocidad); if (steps3 - pasos < 0){ direccion = (direccion == 0)?1:0; motores2(2000,direccion); delay_us(200); motores3(2147483640,direccion); direccion = (direccion == 0)?1:0; motores2(2000,direccion); } break; case '8': printf("Setup Calibracion Quick\n\r"); motor_on(motor); motores3(2147483640,DERECHA); delay_us(200); motores2(2000,IZQUIERDA); delay_us(200); izq_steps = motores3(2147483640,IZQUIERDA); delay_us(200); motores2(2000,DERECHA); delay_us(200); der_steps = motores3(2147483640,DERECHA); printf("izq_steps ->%Ld<- \n\r",izq_steps); printf("der_steps ->%Ld<- \n\r",der_steps); while(true){ motores2(izq_steps,IZQUIERDA); delay_us(200); motores2(der_steps,DERECHA); delay_us(200); } case '9': printf("Setup Velocidad ...\n\r"); output_high(PIN_A4); motores2(2000,IZQUIERDA); delay_us(200); izq_steps = motores3(2147483640,IZQUIERDA); delay_us(200); motores2(2000,DERECHA); delay_us(200); der_steps = motores3(2147483640,DERECHA); printf("izq_steps ->%Ld<- \n\r",izq_steps); printf("der_steps ->%Ld<- \n\r",der_steps); motores4(izq_steps,IZQUIERDA,velocidad); delay_us(200); motores4(der_steps,DERECHA,200); delay_us(200); break; } } } //FIN MAIN
void gesture_processdata(int16_t *input) { int8_t result[3]; if (state == STATE_NONE) return; if (state == STATE_RECON && count > MAX_DATAPOINTS) { PRINTF("No MATCH\n"); process_post(ui_process, EVENT_GESTURE_MATCHED, (void*)0); gesture_shutdown(); return; } //PRINTF("%d,%d,%d,\n", input[0], input[1], input[2]); count++; // integrate into move window average // get rid of oldest for(int i = 2; i >=0; i--) { int16_t currentsum; data[datap][i] = input[i] / MOVE_WINDOW; if ((datap & (MOVE_STEP - 1)) != (MOVE_STEP -1)) continue; currentsum = data[0][i] + data[1][i] + data[2][i] + data[3][i]; // if (count < MOVE_WINDOW) // continue; result[i] = Normalize(currentsum); } datap++; datap &= (MOVE_WINDOW - 1); if ((datap % MOVE_STEP == 0) && (count >= MOVE_WINDOW)) { printf("%d,%d,%d,\n", (int)result[0], (int)result[1], (int)result[2]); // PRINTF("%d,%d,%d,\n", (int)result[0], (int)result[1], (int)result[2]); if (state == STATE_RECORDING) { if (count > MAX_DATAPOINTS) { PRINTF("===\n"); state = STATE_NONE; } } else { //PRINTF("%d %d %d\n", result[0], result[1], result[2]); uint16_t shortestDistance = 0xffff; uint16_t longestDistance = 0; uint8_t bestMatch; uint32_t totalDistance = 0; for(int k = 0; k < NUM_GESTURES; k++) { uint16_t distance = gesture_caculate(k, result); PRINTF("%d => %d\t", k, distance); if (distance < shortestDistance) { shortestDistance = distance; bestMatch = k; } if (distance > longestDistance) { longestDistance = distance; } totalDistance += distance; } uint16_t averageDistance = totalDistance/NUM_GESTURES; PRINTF("ad: %d, sd: %d, ld: %d, var: %d\n", averageDistance, shortestDistance, longestDistance, longestDistance - shortestDistance); if ((shortestDistance > averageDistance / 2) || count < MAX_GESTURES) { PRINTF("almost matched %d\n", bestMatch+1); return; } // matched PRINTF("Matched %d\n", bestMatch+1); motor_on(200, CLOCK_SECOND/4); process_post(ui_process, EVENT_GESTURE_MATCHED, (void*)(bestMatch + 1)); gesture_shutdown(); return; } } }
/*--------------------------------------------------------------------------*/ int main(int argc, char **argv) { /* * Initalize hardware. */ msp430_cpu_init(); clock_init(); uart_init(9600); /* Must come before first printf */ /* xmem_init(); */ PRINTF("iWatch 0.10 build at " __TIME__ " " __DATE__ "\n"); UCSCTL8 &= ~BIT2; /* * Hardware initialization done! */ /* * Initialize Contiki and our processes. */ process_init(); process_start(&etimer_process, NULL); rtimer_init(); ctimer_init(); energest_init(); ENERGEST_ON(ENERGEST_TYPE_CPU); backlight_init(); battery_init(); SPI_FLASH_Init(); if (system_testing()) { clock_time_t t; backlight_on(200, 0); t = clock_seconds(); // sleep 1 while(clock_seconds() - t <= 3); printf("$$OK BACKLIGHT\n"); t = clock_seconds(); while(clock_seconds() - t <= 3); backlight_on(0, 0); motor_on(200, 0); // sleep 1s t = clock_seconds(); while(clock_seconds() - t <= 3); printf("$$OK MOTOR\n"); t = clock_seconds(); while(clock_seconds() - t <= 3); motor_on(0, 0); #if PRODUCT_W001 I2C_Init(); codec_init(); codec_bypass(1); // sleep 1s t = clock_seconds(); while(clock_seconds() - t <= 3); printf("$$OK MIC\n"); // sleep 1s t = clock_seconds(); while(clock_seconds() - t <= 3); codec_bypass(0); codec_shutdown(); #endif } int reason = CheckUpgrade(); window_init(reason); button_init(); rtc_init(); CFSFontWrapperLoad(); system_init(); // check system status and do factor reset if needed I2C_Init(); //codec_init(); //ant_init(); bluetooth_init(); #ifdef PRODUCT_W004 //bmx_init(); #else mpu6050_init(); #endif // check the button status if (button_snapshot() & (1 << BUTTON_UP)) { clock_time_t t; // delay 1 second // button up is pressed, we will set emerging flag motor_on(200, CLOCK_SECOND * 2); t = clock_seconds(); while(clock_seconds() - t <= 1); if (button_snapshot() & (1 << BUTTON_UP)) system_setemerging(); motor_on(0, 0); } if (!system_retail()) { bluetooth_discoverable(1); } #if PRODUCT_W001 if (system_testing()) ant_init(MODE_HRM); #endif system_restore(); // protocol_init(); // protocol_start(1); process_start(&system_process, NULL); /* * This is the scheduler loop. */ msp430_dco_required = 0; /* check firmware update */ if (reason == 0xff) { printf("Start Upgrade\n"); Upgrade(); // never return if sucessfully upgrade } watchdog_start(); while(1) { int r; do { /* Reset watchdog. */ watchdog_periodic(); r = process_run(); } while(r > 0); /* * Idle processing. */ int s = splhigh(); /* Disable interrupts. */ /* uart1_active is for avoiding LPM3 when still sending or receiving */ if(process_nevents() != 0) { splx(s); /* Re-enable interrupts. */ } else { static unsigned long irq_energest = 0; /* Re-enable interrupts and go to sleep atomically. */ ENERGEST_OFF(ENERGEST_TYPE_CPU); ENERGEST_ON(ENERGEST_TYPE_LPM); /* We only want to measure the processing done in IRQs when we are asleep, so we discard the processing time done when we were awake. */ energest_type_set(ENERGEST_TYPE_IRQ, irq_energest); watchdog_stop(); if (shutdown_mode) { system_shutdown(1); // never return LPM4; } if (msp430_dco_required) { __low_power_mode_0(); } else { __low_power_mode_3(); } /* We get the current processing time for interrupts that was done during the LPM and store it for next time around. */ __disable_interrupt(); irq_energest = energest_type_time(ENERGEST_TYPE_IRQ); __enable_interrupt(); watchdog_start(); ENERGEST_OFF(ENERGEST_TYPE_LPM); ENERGEST_ON(ENERGEST_TYPE_CPU); } } }
/* * Called when the user alarm has expired */ void on_user_alarm_expire() { motor_on(); led_start(); }
/* * And now, it's time to implenent the read or write function, that's * all the floppy driver mean! * * Read/Write one sector once. */ static int floppy_rw(int sector, char *buf, int command) { int head; char *dma_buffer = buf; static char tmp_dma_buffer[512]; //LOG("TMP dma buffer: %p\n", tmp_dma_buffer); lba_to_chs(sector, &head, &track, §or); LOG("head: %d \ttrack: %d \tsector: %d\n", head, track, sector); /* turn it on if not */ motor_on(); if (inb_p(FD_DIR) & 0x80) { changed = TRUE; seek(1, head); /* clear "disk change" status */ recalibrate(); motor_off(); printk("floppy_rw: Disk change detected. You are going to DIE:)\n"); pause(); /* just put it in DIE */ } /* move head to the right track */ if (!seek(track, head)) { motor_off(); printk("floppy_rw: Error seeking to track#%d\n", track); return FALSE; } if ((unsigned long)buf >= 0xff000) { dma_buffer = tmp_dma_buffer; if (command == FD_WRITE) memcpy(dma_buffer, buf, 512); } setup_DMA((unsigned long)dma_buffer, command); send_byte(command); send_byte(head<<2 | 0); send_byte(track); send_byte(head); send_byte(sector); send_byte(2); /* sector size = 125 * 2^(2) */ send_byte(floppy.sector); send_byte(0); send_byte(0xFF); /* sector size(only two valid vaules, 0xff when n!=0*/ if (!wait_fdc(FALSE)) { //LOG("wait fdc failed!\n"); //return 0; /* printk("Time out, trying operation again after reset() \n"); reset(); return floppy_rw(sector, buf, command); */ } motor_off(); if (/*res != 7 || */(ST0 & 0xf8) || (ST1 & 0xbf) || (ST2 & 0x73) ) { if (ST1 & 0x02) LOG("Drive is write protected!\n"); else LOG("floppy_rw: bad interrupt!\n"); return -EIO; } else { LOG("floppy_rw: OK\n"); if ((unsigned long)buf >= 0xff000 && command == FD_READ) memcpy(buf, dma_buffer, 512); return 0; } }