// Process incoming status record (update status table and remove from sent_statuses)
void receive_status(struct msgrecord *in) {
int i;
#ifdef DEBUG
Serial.println("Status:");
Serial.print(in->id);
Serial.print(" ");
Serial.println(in->status);
#endif
i = find_status(in->id);
if (i < 0) {
if (numstatuses >= ALLSTATUS_SIZE)
numstatuses = ALLSTATUS_SIZE - 1;
i = numstatuses;
allstatuses[i].id = in->id;
numstatuses++;
}
allstatuses[i].status = in->status;
// Update sent_statuses table so we won't send obsolete status
for (i = 0; i < num_sent; i++) {
if (sent_statuses[i].expire > 0) {
if (sent_statuses[i].id == in->id) {
sent_statuses[i].expire = 0;
break;
}
}
}
if (in->id == 1)
set_all_statuses(in->status);
} // void receive_status(struct msgrecord in)
int
m_restore_object(struct object *ob, struct mapping *map)
{
int p;
int i;
struct apair *j;
if (ob->flags & O_DESTRUCTED)
return 0;
for (i = 0; i < map->size; i++)
{
for (j = map->pairs[i]; j ; j = j->next)
{
if (j->arg.type != T_STRING)
continue;
if ((p = find_status(ob->prog, j->arg.u.string, TYPE_MOD_STATIC))
== -1)
continue;
assign_svalue(&ob->variables[p], &j->val);
}
}
return 1;
}
// Find status in last/lastfull for specific id
status_type check_status(id_type id) {
status_type temp;
int i;
// We use default "master" status if not find specific one
temp = allstatuses[0].status;
if ((i = find_status(id)) >= 0)
temp = allstatuses[i].status;
return temp;
} // status_type check_status(id_type id)
/*
motor1 : LT
motor2 : LB
motor3 : RT
motor4 : RB
motor5 : BACK
motor6 : BACK
*/
int main() {
OSCclass *c=new OSCclass;
OSCmsg *recv;
uint8_t channel = 7;
// Set the Channel. 0 is the default, 15 is max
//mrf.SetChannel(channel);
char add[5];
char *command;
pc.printf("Start----- Haptic Vest!\r\n");
while(1) {
rxLen = rf_receive(rxBuffer, 128);
// pc.printf("RxLen %d", rxLen);
if(rxLen > 0) {
recv= c->getOSCmsg(rxBuffer);
printf("Address is %s with type %c and msg %c \r\n",recv->getAddr(),recv->getType(),recv->getArgs());
strncpy(add,recv->getAddr(),5);
if(add[1] == 'B'){
motorStatus = find_status(add);
pc.printf("motor status: %d\r\n", motorStatus);
motor6 = motorStatus%10;//BACK
//motor5 = motor6;
motor4 = (motorStatus / 10) % 10;//RB
motor3 = (motorStatus / 100) % 10;//RT
//solenoid2=motor3;
motor2 = (motorStatus / 1000) % 10;//LB
motor1 = (motorStatus / 10000) % 10;//LT
//solenoid1=motor1;
//printf("%d %d %d %d %d %d \r\n",motor1,motor2,motor3,motor4,motor5,motor6);
updateMotors();
}
}
}
}
// Send status (slave->master, master->slaves), 0 is error (last one will be resent periodically)
int send_status(id_type id, status_type status) {
struct {
uint16_t nstatuses;
struct msgrecord record;
} sendPacket;
int empty, ind, i;
sendPacket.nstatuses = 1;
sendPacket.record.id = id;
sendPacket.record.status = status;
// Update allstatuses table
ind = find_status(id);
if (ind < 0) {
ind = numstatuses;
numstatuses++;
allstatuses[ind].id = id;
}
allstatuses[ind].status = status;
// Update sent_statuses table
ind = -1;
empty = -1;
for (i = 0; i < num_sent; i++) {
if (sent_statuses[i].expire > 0) {
ind = i;
break;
} else
empty = i;
}
if (ind < 0) {
if (empty < 0) {
empty = num_sent;
num_sent++;
}
ind = empty;
}
sent_statuses[ind].id = id;
sent_statuses[ind].expire = MSGMULTI_RESEND;
if (id == 1) {
set_all_statuses(status);
sent_statuses[ind].expire = 5;
}
#ifdef DEBUG
Serial.print("Sending status, id: ");
Serial.print(id);
Serial.print(", status: ");
Serial.println(status);
#endif
return send_packet((char *) &sendPacket, sizeof(sendPacket), addr_local);
} // int send_status(id_type id, status_type status)
struct svalue
get_variable(struct object *ob, char *var_name)
{
int i;
struct svalue res = const0;
if (ob->flags & O_DESTRUCTED)
return res;
if (!ob->variables)
return const0;
if ((i = find_status(ob->prog, var_name,0)) != -1)
assign_svalue_no_free(&res, &ob->variables[i]);
return res;
}
int main(int argc,char**argv)
{
int i,case_no=0,case_num,num,status;
char s[MAXLEN];
scanf("%d",&case_num);
while(case_num--)
{
getchar();
gets(s);
num=0;
for(i=0;s[i]!='[';i++)
{
num *=10;
num +=(s[i]-'0');
}
status=find_status(s[i+1]);
assert(status!=M_ERROR);
/*输出%需要使用%%*/
printf("Case #%d: %.2lf%%\n",++case_no,100*calc_percentage(num,status));
clear_str(s,MAXLEN);
}
return 0;
}
int
restore_object(struct object *ob, char *file)
{
char *name, var[100], *buff, *space;
size_t len;
FILE *f;
struct object *save = current_object;
struct stat st;
int p;
if (current_object != ob)
fatal("Bad argument to restore_object()\n");
if (ob->flags & O_DESTRUCTED)
return 0;
file = check_valid_path(file, ob, "restore_object", 0);
if (file == 0)
error("Illegal use of restore_object()\n");
len = strlen(file);
name = alloca(len + 3);
(void)strcpy(name, file);
if (name[len-2] == '.' && name[len-1] == 'c')
name[len-1] = 'o';
else
(void)strcat(name, ".o");
f = fopen(name, "r");
if (s_flag)
num_fileread++;
if (!f || fstat(fileno(f), &st) == -1) {
if (f)
(void)fclose(f);
return 0;
}
if (st.st_size == 0) {
(void)fclose(f);
return 0;
}
buff = xalloc((size_t)st.st_size + 1);
current_object = ob;
for (;;) {
struct svalue *v;
if (fgets(buff, (int)st.st_size + 1, f) == 0)
break;
/* Remember that we have a newline at end of buff ! */
space = strchr(buff, ' ');
if (space == 0 || space - buff >= sizeof (var)) {
(void)fclose(f);
free(buff);
error("Illegal format when restoring %s.\n", file);
}
(void)strncpy(var, buff, (size_t)(space - buff));
var[space - buff] = '\0';
p = find_status(ob->prog, var, TYPE_MOD_STATIC);
if (p == -1)
continue;
v = &ob->variables[p];
space++;
if (!restore_one(v, &space)) {
(void)fclose(f);
free(buff);
error("Illegal format when restoring %s from %s.\n", var, file);
}
}
current_object = save;
if (d_flag & DEBUG_RESTORE)
debug_message("Object %s restored from %s.\n", ob->name, file);
free(buff);
(void)fclose(f);
return 1;
}
// Periodically resend statuses
void resend_status() {
int i, ind;
char *p;
uint16_t n;
struct msgrecord tmp;
#ifdef DEBUG
Serial.print("resend_status, timer_resend: ");
Serial.print(timer_resend);
Serial.print(", timer_resend_all: ");
Serial.print(timer_resend_all);
Serial.print(", millis(): ");
Serial.println(millis());
#endif
// Check if we have to resend (every 200 milliseconds)
if ((millis() - timer_resend) > 200) {
timer_resend = millis();
// Construct packet from sent_statuses array
n = 0;
p = packetBuffer + sizeof(uint16_t);
for (i = 0; i < num_sent; i++) {
if (sent_statuses[i].expire > 0) {
sent_statuses[i].expire--;
tmp.id = sent_statuses[i].id;
ind = find_status(tmp.id);
if (ind >= 0) {
tmp.status = allstatuses[ind].status;
memcpy(p, (char *) &tmp, sizeof(struct msgrecord));
p += sizeof(struct msgrecord);
n++;
}
}
}
if (n > 0) {
memcpy(packetBuffer, &n, sizeof(uint16_t));
send_packet(packetBuffer, p - packetBuffer, addr_local);
}
}
// Check if we are master and have to resend all available statuses
if ((my_node_type == MSGMULTI_MASTER) && ((millis() - timer_resend_all) > MSGMULTI_RESEND_ALL)) {
timer_resend_all = millis();
n = 0;
p = packetBuffer + sizeof(uint16_t);
for (i = 0; i < numstatuses; i++) {
// Check if buffer is enough for next one
if ((p - packetBuffer) > (sizeof(packetBuffer) - sizeof(struct msgrecord)))
break;
memcpy(p, &allstatuses[i], sizeof(struct msgrecord));
p += sizeof(struct msgrecord);
n++;
}
#ifdef DEBUG
Serial.print("n: ");
Serial.print(n);
Serial.print(", numstatuses: ");
Serial.println(numstatuses);
#endif
if (n > 0) {
memcpy(packetBuffer, &n, sizeof(uint16_t));
send_packet(packetBuffer, p - packetBuffer, addr_local);
}
}
} // void resend_status()
