void set_Arm (unsigned char mode)
{
#ifdef CMM_ARM_EXPERIMENT
unsigned long cur_pos;
unsigned char old_mode;
unsigned char pct_deployed;
get_ArmPosition(&cur_pos, &old_mode);
if (mode == old_mode) return;
if (old_mode != ARM_STOP)
{
if (cur_pos != arm_pos)
{
pct_deployed = cur_pos / ARM_STROKE_PCT;
#ifdef ARM_POS_DEBUG
DBG("Arm moved ");
if (cur_pos > arm_pos)
DBG2("+%lu", cur_pos - arm_pos);
else
DBG2("-%lu", arm_pos - cur_pos);
DBG3(" to %lu (%lu)\n", cur_pos, pct_deployed);
#endif
arm_pos = cur_pos;
}
}
else
{
pct_deployed = cur_pos / ARM_STROKE_PCT;
}
if (mode != ARM_STOP) gettimestamp(&arm_start);
#endif
switch (mode) {
default:
case ARM_STOP:
ARM(LAT) &= ~(ARM_UPDOWN_MASK | ARM_ONOFF_MASK);
break;
case ARM_UP:
ARM(LAT) &= ~ARM_UPDOWN_MASK;
ARM(LAT) |= ARM_ONOFF_MASK;
break;
case ARM_DOWN:
ARM(LAT) |= ARM_UPDOWN_MASK;
ARM(LAT) |= ARM_ONOFF_MASK;
break;
}
#ifdef CMM_ARM_EXPERIMENT
// % deployed in lower bits, mode in upper bits
eventlog_track(EVENTLOG_ARM, (((uint16_t)pct_deployed) << 8) | mode);
#else
eventlog_track(EVENTLOG_ARM, mode);
#endif
}
void ertictac_state::ertictac(machine_config &config)
{
ARM(config, m_maincpu, XTAL(24'000'000)/3); /* guess, 12MHz 8MHz or 6MHz, what's the correct divider 2, 3 or 4? */
m_maincpu->set_addrmap(AS_PROGRAM, &ertictac_state::ertictac_map);
m_maincpu->set_periodic_int(FUNC(ertictac_state::ertictac_podule_irq), attotime::from_hz(60)); // FIXME: timing of this
I2CMEM(config, "i2cmem", 0).set_page_size(NVRAM_PAGE_SIZE).set_data_size(NVRAM_SIZE);
// AAKART(config, m_kart, XTAL(24'000'000)/3); // TODO: frequency
SCREEN(config, m_screen, SCREEN_TYPE_RASTER);
m_screen->set_raw(XTAL(16'000'000), 1024,0,735, 624/2,0,292); // RiscOS 3 default screen settings
m_screen->set_screen_update(FUNC(archimedes_state::screen_update));
PALETTE(config, m_palette).set_entries(0x200);
SPEAKER(config, "speaker").front_center();
for (int i = 0; i < 8; i++)
{
DAC_16BIT_R2R_TWOS_COMPLEMENT(config, m_dac[i], 0).add_route(0, "speaker", 0.05); // unknown DAC
}
voltage_regulator_device &vref(VOLTAGE_REGULATOR(config, "vref", 0));
vref.add_route(0, "dac0", 1.0, DAC_VREF_POS_INPUT); vref.add_route(0, "dac0", -1.0, DAC_VREF_NEG_INPUT);
vref.add_route(0, "dac1", 1.0, DAC_VREF_POS_INPUT); vref.add_route(0, "dac1", -1.0, DAC_VREF_NEG_INPUT);
vref.add_route(0, "dac2", 1.0, DAC_VREF_POS_INPUT); vref.add_route(0, "dac2", -1.0, DAC_VREF_NEG_INPUT);
vref.add_route(0, "dac3", 1.0, DAC_VREF_POS_INPUT); vref.add_route(0, "dac3", -1.0, DAC_VREF_NEG_INPUT);
vref.add_route(0, "dac4", 1.0, DAC_VREF_POS_INPUT); vref.add_route(0, "dac4", -1.0, DAC_VREF_NEG_INPUT);
vref.add_route(0, "dac5", 1.0, DAC_VREF_POS_INPUT); vref.add_route(0, "dac5", -1.0, DAC_VREF_NEG_INPUT);
vref.add_route(0, "dac6", 1.0, DAC_VREF_POS_INPUT); vref.add_route(0, "dac6", -1.0, DAC_VREF_NEG_INPUT);
vref.add_route(0, "dac7", 1.0, DAC_VREF_POS_INPUT); vref.add_route(0, "dac7", -1.0, DAC_VREF_NEG_INPUT);
}
unsigned char get_Arm (void)
{
switch (ARM(LAT) & (ARM_UPDOWN_MASK | ARM_ONOFF_MASK)) {
case ARM_ONOFF_MASK:
return (ARM_UP);
case ARM_ONOFF_MASK | ARM_UPDOWN_MASK:
return (ARM_DOWN);
}
return (ARM_STOP);
}
set_name("Kallin") ;
credit(random(300)+ 102) ;
set_level(30) ;
set_skill("Evasion", 2, "dexterity") ;
set_skill("Awareness", 2, "dexterity") ;
set_skill("Clubbing Weapons", 2, "strength") ;
set("stat", ([
"strength" : 13 , "dexterity" : 16,
"constitution" : 17, "intelligence" : 17,
"wisdom" : 9, "charisma" : 10
]) ) ;
set_alignment("cn") ;
set("damage", ({ 2, 4 }) );
set("weapon_name", "fists");
set_verbs( ({ "swing at", "attack" }) );
wear(ARM(protring3), "ring") ;
wear(ARM(elven_chain), "chain") ;
wear(ARM(displacercloak), "cloak") ;
wear(ARM(boots_of_speed), "boots") ;
arm(WEP(warstaff), "staff") ;
set("special", 1) ;
}
void special_attack() {
object victim;
string v_msg, msg, name, damage_type ;
int damage ;
if (random(10) < 5) return ;
victim = query_current_attacker() ;
if (!victim) return ;
if (environment(TO) != environment(victim)) return ;
set_skill("Two Weapon Style", 2, "strength") ;
set("long", @ENDLONG
Fernelius looks like the type of individual you'd expect to see
trying to rob someone in a busy marketplace. He looks like he
can handle himself in combat, however.
ENDLONG
);
set("id",({ "fern", "fernelius" }) ) ;
set("gender", "male");
set("race", "half-elf");
set_name("Fernelius") ;
credit(random(300)+ 102) ;
set_level(30) ;
set("stat", ([
"strength" : 15, "dexterity" : 19, "intelligence" : 12,
"wisdom" : 11, "charisma" : 9, "constitution" : 10
]) ) ;
set_skill("Ambidexterity", 1, "dexterity") ;
set_skill("Short Blades", 2, "strength") ;
set_alignment("cn") ;
set("damage", ({ 2, 4 }) );
set("weapon_name", "fists");
set_verbs( ({ "swing at", "attack" }) );
wear(ARM(protring3), "ring") ;
wear(ARM(studded_leather), "leather") ;
arm(WEP(poison_dagger), "dagger") ;
arm(WEP(poison_dagger), "dagger") ;
arm(OBJ(bag_of_holding), "hi there.") ;
wear(ARM(slboots), "boots") ;
}
/**
* Compute the 3D pose in 6DOF using to camera for mutual localization
*
* \param pixelA1 Position of the left LED on robot A
* \param pixelA2 Position of the right LED on robot A
* \param pixelB1 Position of the left LED on robot B
* \param pixelB2 Position of the right LED on robot
* \param position (Output) the position vector
* \param rotation (Output) the rotation matrix
*
* \return the rotation matrix of the relative pose
*
*/
void MutualPoseEstimation::compute3DMutualLocalisation(const Eigen::Vector2d &lPixelA1, const Eigen::Vector2d &lPixelA2,
const Eigen::Vector2d &lPixelB1,const Eigen::Vector2d &lPixelB2,
Eigen::Vector3d &position, Eigen::Matrix3d &rotation){
Eigen::Vector2d fCamA = this->focalCam;
Eigen::Vector2d fCamB = this->focalCam;
Eigen::Vector2d ppA, pixelA1, pixelA2, pixelB1, pixelB2;
if(ConventionUV){ // Oliver's code use the uv convention ( u point left and v point up)
ppA = this->centerCam;
pixelA1 = lPixelA1;
pixelA2 = lPixelA2;
pixelB1 = lPixelB1;
pixelB2 = lPixelB2;
}
else{
ppA = fromXYCoordinateToUVCoordinate(this->centerCam);
pixelA1 = fromXYCoordinateToUVCoordinate(lPixelA1);
pixelA2 = fromXYCoordinateToUVCoordinate(lPixelA2);
pixelB1 = fromXYCoordinateToUVCoordinate(lPixelB1);
pixelB2 = fromXYCoordinateToUVCoordinate(lPixelB2);
}
Eigen::Vector2d ppB = ppA;
//Eigen::Vector2d ppA = 0;
//Eigen::Vector2d ppB = 0;
/*cout<<"-Parameters-"<<endl;
cout<<"pixelA1:"<<pixelA1<<endl;
cout<<"pixelA2:"<<pixelA2<<endl;
cout<<"pixelB1:"<<pixelB1<<endl;
cout<<"pixelB2:"<<pixelB2<<endl;
cout<<"ppA:"<<ppA<<endl;
cout<<"ppB:"<<ppB<<endl;
cout<<"fCamA:"<<fCamA<<endl;
cout<<"fCamB:"<<fCamB<<endl;
cout<<"rdA:"<<rdA<<endl;
cout<<"ldA:"<<ldA<<endl;
cout<<"rdB:"<<rdB<<endl;
cout<<"ldB:"<<ldB<<endl;*/
//Eigen::Vector3d PAM1, PAM2;
//if (ConventionUV) {
Eigen::Vector3d PAM1((pixelB1[0]-ppB[0])/fCamB[0], (pixelB1[1]-ppB[1])/fCamB[1], 1);
Eigen::Vector3d PAM2((pixelB2[0]-ppB[0])/fCamB[0], (pixelB2[1]-ppB[1])/fCamB[1], 1);
// }
// else {
// // Convention x-y
// PAM1((ppB[0]-pixelB1[0])/fCamB[0], (ppB[1]-pixelB1[1])/fCamB[1], 1);
// PAM2((ppB[0]-pixelB2[0])/fCamB[0], (ppB[1]-pixelB2[1])/fCamB[1], 1);
// }
PAM1.normalize();
PAM2.normalize();
double alpha = acos(PAM1.dot(PAM2));
//printf("Alpha: %f\n",alpha);
double d = this->rdA + this->ldA;
Eigen::Vector2d BLeftMarker = pixelA2;
Eigen::Vector2d BRightMarker = pixelA1;
Eigen::Vector2d PB1(BLeftMarker[0] + (this->ldB/(rdB+ldB)) * (BRightMarker[0] - BLeftMarker[0]),
BLeftMarker[1] + (this->ldB/(rdB+ldB)) * (BRightMarker[1] - BLeftMarker[1]));
Eigen::Vector3d PB12((PB1[0]-ppA[0])/fCamA[0], (PB1[1]-ppA[1])/fCamA[1], 1);
PB12.normalize();
double phi = acos(PB12[0]);
double beta = 0.5f * M_PI - phi;
//printf("Beta: %f\n",beta* 180.f/M_PI);
Eigen::Vector2d plane = MutualPoseEstimation::computePositionMutual(alpha, beta, d);
double EstimatedDistance = plane.norm();
position = PB12 * EstimatedDistance;
//====================================================================
//=========================Axis Angle Part============================
//Create the two plans
//Plan in B Refs
Eigen::Vector2d ALeftMarker = pixelB2;
Eigen::Vector2d ARightMarker = pixelB1;
Eigen::Vector3d ALM((ALeftMarker[0]-ppB[0])/fCamB[0], (ALeftMarker[1]-ppB[1])/fCamB[1], 1);
ALM.normalize();
Eigen::Vector3d ARM((ARightMarker[0]-ppB[0])/fCamB[0], (ARightMarker[1]-ppB[1])/fCamB[1], 1);
ARM.normalize();
//Plan in A Refs
Eigen::Vector3d AToB = PB12;
Eigen::Vector3d LeftMarker(1, 0, 0);
//Align the two plans
Eigen::Vector3d NormalPlanInB = ALM.cross(ARM);
Eigen::Vector3d NormalPlanInA = AToB.cross(LeftMarker);
Eigen::Vector3d AxisAlignPlans = NormalPlanInB.cross(NormalPlanInA);
NormalPlanInB.normalize();
NormalPlanInA.normalize();
AxisAlignPlans.normalize();
double AngleAlignPlans = acos(NormalPlanInB.dot(NormalPlanInA));
Eigen::MatrixXd AlignPlans = vrrotvec2mat(AngleAlignPlans, AxisAlignPlans);
//Align the vector of the cameraA seen from B with the plan
Eigen::Vector3d CameraASeenFromB(
((ALeftMarker[0] + (this->ldA/(this->rdA+this->ldA))*(ARightMarker[0] - ALeftMarker[0]))-ppB[0])/fCamB[0],
((ALeftMarker[1] + (this->ldA/(this->rdA+this->ldA))*(ARightMarker[1] - ALeftMarker[1]))-ppB[1])/fCamB[1],
1);
CameraASeenFromB.normalize();
Eigen::Vector3d alignedBToA = AlignPlans * CameraASeenFromB;
//Turn the vector BToA to make it align with AToB
Eigen::Vector3d AxisAlignABwBA = alignedBToA.cross(AToB);
AxisAlignABwBA.normalize();
//Since we know that cameras are facing each other, we rotate pi
double AngleAlignABwBA = acos(alignedBToA.dot(AToB)) - M_PI;
Eigen::Matrix3d AlignVectors = vrrotvec2mat(AngleAlignABwBA, AxisAlignABwBA);
rotation = AlignVectors * AlignPlans;
}
keep the stables clean.
ENDLONG
);
set("id", ({ "stable boy", "boy" }) );
set("gender", "male");
set("race", "human");
set_name("stable boy");
credit(random(20)) ;
set_level(3);
set_alignment("cg") ;
set("damage", ({ 2, 4 }) );
set_size( 6);
set("weapon_name", "fists");
set_verbs( ({ "swing at", "attack" }) );
arm(WEP(pitchfork), "fork");
wear(ARM(overalls), "overalls");
set("chat_chance", 6);
set("chat_output", ({
"The boy whistles a little ditty.\n",
"The boy wipes some manure on his overalls.\n",
"The boy pitches some hay.\n",
"The boy slips in some manure and curses under his breath.\n",
"The boy pitches some hay and wipes the sweat from his brow.\n",
}) );
set("att_chat_output", ({
"The boy cries out in pain!\n",
"The boy slings some manure at you!\n",
}) );
}
int raspi_map_hw(void)
{
int fd = open("/dev/mem", O_RDWR|O_SYNC);
if (fd < 0) return 0;
void *addr = mmap(NULL, sizeof(raspi_peripherals), PROT_READ|PROT_WRITE, MAP_SHARED|MAP_LOCKED|MAP_POPULATE, fd, ARM(0));
close(fd);
if (addr == MAP_FAILED) return 0;
pHW = (raspi_peripherals *)addr;
return 1;
}
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#include "hw.h"
raspi_peripherals *pHW = (raspi_peripherals *)ARM(0);
#if defined(linux) && !defined(__KERNEL__)
#include <unistd.h>
#include <fcntl.h>
#include <sys/mman.h>
int raspi_map_hw(void)
{
int fd = open("/dev/mem", O_RDWR|O_SYNC);
if (fd < 0) return 0;
void *addr = mmap(NULL, sizeof(raspi_peripherals), PROT_READ|PROT_WRITE, MAP_SHARED|MAP_LOCKED|MAP_POPULATE, fd, ARM(0));
close(fd);
