static SQInteger erase_wait(HSQUIRRELVM v)
{
struct program_config *d;
SQRESULT r = qr_userpointer_get(v, (SQUserPointer) &d);
if(SQ_FAILED(r)){
return r;
}
if(0){
long timer_wait = erase_timer_get(d->handle, &d->cpu);
long timer_ppu = erase_timer_get(d->handle, &d->ppu);
if(timer_wait < timer_ppu){
timer_wait = timer_ppu;
}
wait_msec(timer_wait);
}else{
uint8_t s[2];
do{
wait_msec(2);
d->control->flash_status(d->handle, s);
//本来の意図からではここの条件式は && ではなく || だが、先に erase が終わったデバイスが動かせるので残しておく
}while((s[0] != KAZZO_TASK_FLASH_IDLE) && (s[1] != KAZZO_TASK_FLASH_IDLE));
}
return 0;
}
errval_t ti_twl6030_set_vmmc_vsel(int millis)
{
TWL_DEBUG("ti_twl6030_vmmc_vsel\n");
//ti_twl6030_mmcctrl_vmmc_auto_off_wrf(&twl, 0x0);
ti_twl6030_mmcctrl_sw_fc_wrf(&twl, 0x1);
ti_twl6030_vmmc_off();
wait_msec(10);
ti_twl6030_vsel_t vsel = millis_to_vsel(millis);
ti_twl6030_vmmc_cfg_voltage_vsel_wrf(&twl, vsel);
ti_twl6030_vmmc_on();
//ti_twl6030_mmcctrl_vmmc_auto_off_wrf(&twl, 0x0);
ti_twl6030_vmmc_pr();
return SYS_ERR_OK;
}
static SQInteger program_main(HSQUIRRELVM v)
{
if(sq_gettop(v) != (1 + 3)){ //roottable, userpointer, co_cpu, co_ppu
return sq_throwerror(v, wgT("argument number error"));
}
struct program_config *d;
SQRESULT r = qr_userpointer_get(v, (SQUserPointer) &d);
if(SQ_FAILED(r)){
return r;
}
HSQUIRRELVM co_cpu, co_ppu;
if(SQ_FAILED(sq_getthread(v, 3, &co_cpu))){
return sq_throwerror(v, wgT("thread error"));
}
if(SQ_FAILED(sq_getthread(v, 4, &co_ppu))){
return sq_throwerror(v, wgT("thread error"));
}
SQInteger state_cpu = sq_getvmstate(co_cpu);
SQInteger state_ppu = sq_getvmstate(co_ppu);
const long sleepms = d->compare == true ? 6 : 2; //W29C040 で compare をすると、error が出るので出ない値に調整 (やっつけ対応)
while((state_cpu != SQ_VMSTATE_IDLE) || (state_ppu != SQ_VMSTATE_IDLE)){
uint8_t s[2];
wait_msec(sleepms);
d->control->flash_status(d->handle, s);
if(state_cpu != SQ_VMSTATE_IDLE && s[0] == KAZZO_TASK_FLASH_IDLE){
if(program_memoryarea(co_cpu, d->handle, &d->cpu, d->compare, &state_cpu, &d->log) == false){
//sq_pushbool(v, SQFalse);
return 0;
}
}
if(state_ppu != SQ_VMSTATE_IDLE && s[1] == KAZZO_TASK_FLASH_IDLE){
if(program_memoryarea(co_ppu, d->handle, &d->ppu, d->compare, &state_ppu, &d->log) == false){
//sq_pushbool(v, SQFalse);
return 0;
}
}
}
//sq_pushbool(v, SQTrue);
return 0;
}
void main() {
byte SERVO_0 = 0, SERVO_1 = 0;
byte MOTOR_L = 0, MOTOR_R = 0, MOTOR_V = 0;
short BUCKET_L = 0, BUCKET_R = 0, BUCKET_V = 0;
byte T = 0;
init();
while (1) {
byte cmd = serial_rx();
byte power, s0, s1;
switch (cmd) {
case CMD_SET_MOTORS :
MOTOR_L = serial_rx();
MOTOR_R = serial_rx();
MOTOR_V = serial_rx();
s0 = serial_rx();
s1 = serial_rx();
set_servo(1 << 0, SERVO_0);
set_servo(1 << 1, SERVO_1);
SERVO_0 = s0;
SERVO_1 = s1;
// read temperature data from RE2 (ANS7)
serial_tx(read_analog(7));
break;
}
BUCKET_L += MOTOR_L - 127;
BUCKET_R += MOTOR_R - 127;
BUCKET_V += MOTOR_V - 127;
power = 0;
if (BUCKET_L >= 127) {
BUCKET_L -= 127;
power = power | (1 << 0);
}
else if (BUCKET_L < -127) {
BUCKET_L += 127;
power = power | (1 << 1);
}
if (BUCKET_R >= 127) {
BUCKET_R -= 127;
power = power | (1 << 2);
}
else if (BUCKET_R < -127) {
BUCKET_R += 127;
power = power | (1 << 3);
}
if (BUCKET_V >= 127) {
BUCKET_V -= 127;
power = power | (1 << 4);
}
else if (BUCKET_V < -127) {
BUCKET_V += 127;
power = power | (1 << 5);
}
PORTA = power;
wait_msec(4);
}
}
