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;
}
}
